Positioning method and map construction method

Abstract

The invention provides a positioning method, which comprises the following steps: acquiring original point cloud data when a target object moves, and extracting a feature index; fusing the persistent features and the intensity features of the original point cloud data, constructing an intensity distribution index function, and establishing a preset effective feature point filtering threshold in parallel to remove dynamic points in a feature point set; and taking a function value of the intensity distribution index function as a weight value, and obtaining a final pose estimation value of the target object by minimizing distance weighting and processing from the edge feature point to the global map line feature and from the plane feature point to the global map surface feature. According to the positioning method provided by the invention, the intensity information of the point cloud data is fused to optimize the residual error, the spatial distribution of the point cloud is considered, and the intensity attribute of the point cloud is incorporated into calculation, so that the loss function is more refined, and the final pose of the target object can be more accurately estimated by utilizing the transformation matrix through the optimization process of the loss function.

Description

Technical Field

[0001] The present application relates to the field of automatic navigation and positioning technology, and more specifically, to a positioning method and a map construction method. Background Art

[0002] At present, with the rapid development of LiDAR technology and computer vision technology, LiDAR SLAM (Simultaneous Localization and Mapping, referred to as SLAM, i.e., simultaneous localization and mapping) is widely used in autonomous vehicles and service-oriented autonomous vehicles due to its high precision and stability. However, existing SLAM algorithms still face the problems of error accumulation and height drift in complex environments, especially when running for a long time or over a large range. These problems will significantly reduce the accuracy and robustness of the SLAM system. In dynamic and complex environments, sensors such as LiDAR may collect a large number of dynamic noise points and invalid points. These noise data will seriously interfere with the registration accuracy of the SLAM algorithm, thereby affecting its navigation and task execution.

[0003] In the process of processing point cloud data and estimating the pose of autonomous vehicles, the registration method of scanning frames to local maps is simple and fast, but its inherent defect is the rapid accumulation of errors, which will lead to significant degradation of mapping accuracy for long-term systems. Each registration depends only on the relative transformation between consecutive scanning frames, and any deviation from a single measurement may be amplified in subsequent registrations, thus accumulating into considerable errors on a global scale. Although traditional loss functions and loop closure detection techniques can alleviate these problems to a certain extent, they are mainly based on geometric features, which limits their application effect in complex environments. Summary of the invention

[0004] In response to at least one defect or improvement need in the prior art, the present application provides a positioning method and a map construction method for at least improving positioning accuracy or map construction quality.

[0005] To achieve the above objectives, in a first aspect, the present application provides a positioning method, comprising:

[0006] Obtain the original point cloud data of the target object when it moves and extract feature indicators;

[0007] The persistent features and intensity features of the original point cloud data are integrated to construct an intensity distribution index function, and a preset effective feature point filtering threshold is established in conjunction to remove dynamic points from the feature point set;

[0008] The function value of the intensity distribution index function is used as a weight value, and the final pose estimation value of the target object is obtained by minimizing the weighted sum of the distances from the edge feature points to the global map line features and the distances from the plane feature points to the global map surface features.

[0009] Furthermore, the method of fusing the persistent features and intensity features of the original point cloud data, constructing an intensity distribution index function, and simultaneously setting a preset effective feature point filtering threshold to remove dynamic points from the feature point set includes:

[0010] The expression of the intensity distribution index function constructed includes:

[0011]

[0012] in, represents the intensity distribution index value, represents a feature point in the local map M in the k0th scanning frame, k represents the number of the scanning frame; ω P and ω I They represent the weight values ​​of the persistence characteristic index and the intensity distribution index respectively; represents the persistent feature index value, γ∈[0,1] and τ represent parameters used to measure the influence of the time interval on the persistent feature index value, Indicates whether there is a feature point in the τth frame that can be compared with Matching evaluation value; (σ i -θ i ) represents the strength characteristic index value, σ i Represents the intensity distribution value of the nearest neighbor of the feature point, θ i Indicates the intensity threshold of valid feature points;

[0013] like Then the feature point at this time Identify as a valid feature point and retain it; if Then the feature point at this time Identify as invalid feature points and remove them; θ pi Indicates the effective feature point filtering threshold.

[0014] Furthermore, the expression of the final pose estimation value of the target object includes:

[0015]

[0016] in, Represents the final pose estimation value of the target object; Indicates the intensity distribution index value of the neighboring points of the edge feature point; Represents the intensity distribution index value of the neighboring points of the plane feature point; f e (pe ) represents the edge feature point p e Distance to global map line feature; f s (p s ) represents the plane feature point p s The distance to the global map surface feature.

[0017] Furthermore, the edge feature point p e Expressions for distances to global map line features include:

[0018]

[0019]

[0020] Among them, T k represents the pose transformation matrix of the target object at the kth frame compared with the previous frame; n e and They represent the normal vector and position of the line where the edge feature point is located when the covariance matrix of the nearest neighbor point in the global map takes the maximum eigenvalue; p n is a unit vector;

[0021] The plane feature point p s Expressions for distances to global map surface features include:

[0022]

[0023] Among them, n s and They respectively represent the normal vector and position of the plane where the plane feature point is located when the covariance matrix of the nearest neighbor point in the global map takes the minimum eigenvalue.

[0024] Furthermore, the expression of the matching evaluation value includes:

[0025]

[0026] Among them, E k represents the set of edge feature points of the kth frame; S k N represents the set of plane feature points of the kth frame; k (p k ) indicates p k The set of points corresponding to the local map M; when When , it means that there is a feature point in the kth frame that can be compared with Matching, feature points that can be continuously observed in multiple consecutive frames are considered as persistent feature points;

[0027] The expression of the intensity distribution value of the nearest neighbor point of the feature point includes:

[0028]

[0029] Among them, Ni represents the characteristic point The number of neighboring points found by the nearest neighbor search in the local map M; ηi represents the intensity value of the corresponding point in the point cloud.

[0030] In a second aspect, the present application provides a map construction method, comprising:

[0031] Based on the final pose estimation value obtained by any of the above items, by scanning the frame to the local map registration, utilizing the spatial relationship of the point cloud, based on the fusion of height features and intensity features, effective modeling and representation of the surrounding environment is achieved, and a lidar SLAM model based on intensity information optimization loop is constructed;

[0032] The original point cloud data obtained when the target object moves is input into the laser radar SLAM model based on the intensity information optimization loop, and the final point cloud map is output.

[0033] Furthermore, the registration of the scanned frame to the local map, utilizing the spatial relationship of the point cloud, and based on the fusion of the height feature and the intensity feature, realizing effective modeling and representation of the surrounding environment includes:

[0034] Perform weighted summation on the representative height value and the representative intensity mean value of the points in the fan ring unit box to obtain the characteristic value of the fan ring unit box;

[0035] Based on the characteristic value of the fan ring unit box, the signal strength of each scanning point is extracted and sorted to obtain N r *N s The size of the matrix T representing the scan context information; where N r and N s Respectively represent the number of equally divided rings in the radial direction and the number of equally divided sectors in the circumferential direction of the point cloud in a single scanning frame in polar coordinates; each element a of the matrix T i j represents the signal strength at the i-th row and column position;

[0036] Compare the scanning context information of the current frame with the scanning context information of the historical frames to construct a scanning context pair;

[0037] Obtaining a distance value of the scanning context pair based on a cosine distance between a current frame feature descriptor and a historical frame feature descriptor;

[0038] According to the distance value of the scanning context pair, the similarity between the scanning context information of the current frame and the scanning context information of the historical frame is evaluated to obtain a corresponding evaluation result.

[0039] Furthermore, the expression of the characteristic value of the fan ring unit box includes:

[0040] f(P ij )=Φ(P ij )+ω T I′ max (p);

[0041] Among them, P ij represents the set of points in the sector ring unit box composed of the i-th ring and the j-th sector; f(P ij ) represents the characteristic value of the fan ring unit box; Φ(P ij ) represents the fan ring unit box P ij The maximum height value of the inner point is the representative height value; I' max (p) represents the maximum intensity mean of a preset number of nearest neighbor points, which is the representative intensity mean; ω T I′ max The weight value of (p).

[0042] Further, including:

[0043] Eliminate points in the fan ring unit box whose intensity values ​​are lower than the preset intensity threshold;

[0044] Performing a neighbor point search for the remaining points in the fan ring unit box, taking the preset number of nearest neighbor points one by one and solving their corresponding intensity means respectively;

[0045] The largest one among these intensity means is selected as the intensity characteristic value of the sector ring unit box.

[0046] Further, including:

[0047] Traverse the number of feature points in different circles of the current scan frame, and generate a feature distribution vector according to the number of feature points; the expression of the feature distribution vector is:

[0048]

[0049]

[0050] Among them, ||r i ||0 means the radius in the current scan frame is r i The number of non-empty fan ring unit boxes in the ring;

[0051] Compare the components in the feature distribution vector to select historical frames that have a difference with the current scan frame exceeding a preset threshold;

[0052] Compare the scanning context information of the current frame with the scanning context information of the filtered historical frames to construct a scanning context pair;

[0053] Based on the cosine distance between the current frame feature descriptor and the filtered historical frame feature descriptor, the distance value of the scanning context pair is obtained, and its expression includes:

[0054]

[0055] Among them, T q represents the feature descriptor of the current frame, T c is the feature descriptor of the filtered historical frame; It is T q The jth column of It is T c The jth column of and are the corresponding norms respectively; d(T q , T c ) represents the distance value of the scanning context pair.

[0056] In general, the above technical solutions conceived by the present application can achieve the following beneficial effects compared with the prior art:

[0057] (1) The positioning method of the present application integrates the intensity information of point cloud data to optimize the residual, takes into account the spatial distribution of the point cloud, and incorporates the intensity attribute of the point cloud into the calculation, making the loss function more refined. Thus, the transformation matrix can be used through the optimization process of the loss function to more accurately estimate the final pose of the target object, thereby improving the positioning accuracy of the target object.

[0058] (2) The map construction method of the present application is based on the final pose estimation value obtained, and by registering the scanned frame to the local map, the spatial relationship of the point cloud is used to fuse the intensity features with the traditional geometric features, and a fusion feature descriptor is constructed to achieve effective modeling and representation of the surrounding environment. These rich fusion feature descriptors enable the algorithm to perform matching operations more accurately in complex environments, effectively perform loop closure, improve the discrimination of feature points, and enhance the SLAM system's ability to recognize the environment, thereby reducing error accumulation.

[0059] (3) The map construction method of the present application can obtain a quantitative index for evaluating the similarity between the scanning context information of the two by calculating the cosine distance between the current frame feature descriptor and the historical frame feature descriptor. This similarity measurement method takes into account both the quantity distribution of features and the relative relationship between them, thereby improving the accuracy and robustness of loop detection. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in 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 creative work.

[0061] Figure 1 A core flow chart of a positioning method provided in an embodiment of the present application;

[0062] Figure 2 A schematic diagram of a fan ring unit box division provided in an embodiment of the present application;

[0063] Figure 3 A schematic diagram of solving the height characteristic value of a fan ring unit box provided in an embodiment of the present application;

[0064] Figure 4 A schematic diagram of solving the strength characteristic value of a fan ring unit box provided in an embodiment of the present application;

[0065] Figure 5 A mapping result of a KITTI dataset provided in an embodiment of the present application. DETAILED DESCRIPTION

[0066] In order to make the purpose, technical solutions and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. In addition, the technical features involved in each embodiment of the present application described below can be combined with each other as long as they do not conflict with each other.

[0067] The terms "including" or "having" and any variations thereof in the specification, claims or drawings of the present application are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device including a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products or devices.

[0068] As described in the background technology section of the specification, although the traditional loss function and loop detection technology can alleviate the technical problem of insufficient mapping accuracy to a certain extent, it is mainly based on geometric features and fails to fully utilize the intensity information in the lidar data, thus limiting its application effect in complex environments. In view of this, the present application proposes a positioning method and a map construction method for at least improving positioning accuracy or mapping quality.

[0069] refer to Figure 1 An embodiment of the present application proposes a positioning method, which may mainly include the following steps.

[0070] Step 1: Obtain the original point cloud data of the target object when it moves and extract feature indicators.

[0071] In some embodiments, specifically, original target point cloud data identified by an autonomous driving vehicle or other target object during the autonomous driving process is collected, and some corresponding feature indicators (geometric features, intensity features, etc.) are extracted.

[0072] Step 2: Fuse the persistent features and intensity features of the original point cloud data, construct an intensity distribution index function, and simultaneously set a preset effective feature point filtering threshold to remove dynamic points from the feature point set to reduce the number of feature points.

[0073] In some embodiments, specifically, first, the collected surrounding point cloud data is calibrated to obtain accurate intensity information. Then, the intensity value of each point in the point cloud is normalized to be within the intensity value range of [0, 100], and the normalized intensity value is stored in the blue channel of the point cloud data structure of PointXYZRGB type. Let M be the set of feature points in the local map, It represents a feature point in the local map M in the k0-th frame scan. The feature point of the k0-th frame is defined as follows:

[0074]

[0075] Among them, E k represents the set of edge feature points of the kth frame, S k Represents the set of plane feature points of the kth frame, N k (p k ) indicates p k The set of points corresponding to M. Specifically, when When , it means that there is a feature point in the kth frame that can be compared with Only feature points that can be continuously observed in multiple consecutive frames can be considered as persistent feature points.

[0076] When k>k0, for the feature point in the kth frame The persistent characteristic index I value is defined as:

[0077]

[0078] Among them, γ∈[0,1] and τ are used to measure the impact of the time interval on the persistent feature index I. Specifically, when the point cloud distribution near the autonomous driving car is very dense in the τth frame, as the autonomous driving car moves, k gradually increases, causing the point cloud distribution near the location of the autonomous driving car in the previous kth frame to gradually become sparse. Therefore, γ k+1-τ It will gradually approach 0, which can well reflect the distribution of point clouds at the same position in different scanning frames.

[0079] For each feature point Perform nearest neighbor search in the local map, and the neighboring points searched are N i ∈M, where N i To represent the number of neighboring points, η i Represents the intensity value of the corresponding point in the point cloud. The intensity distribution value of the nearest neighbor point of the feature point is σ i The definition is as follows:

[0080]

[0081] For each feature point Its strength characteristic index value P is defined as follows:

[0082] P i =σ i -θ i

[0083] Among them, θ i It is the intensity threshold for identifying valid feature points.

[0084] For each feature point in the local map, the intensity distribution index PI value of its surrounding neighboring points is defined as:

[0085]

[0086] Among them, ω P and ω I They represent the weight values ​​of the persistence feature index and the intensity distribution index respectively. By setting θ pi is the effective feature point filtering threshold, if Then the feature point at this time Identify as a valid feature point and retain it; if Then the feature point at this time Identify invalid feature points and remove them.

[0087] Step 3: Use the function value of the intensity distribution index function as the weight value, and obtain the final pose estimation value of the target object by minimizing the weighted sum of the distances from the edge feature points to the global map line features and the distances from the plane feature points to the global map surface features.

[0088] Specifically, the intensity distribution near the feature points is calculated, and an intensity distribution index is assigned to each feature point. An improved loss function method (constructed into an intensity-weighted loss function) is proposed, which aims to accurately estimate the posture changes of the autonomous driving vehicle at each moment through the optimization process of the loss function, integrate the intensity information of the point cloud data to optimize the residual, and obtain more accurate posture information.

[0089] In some embodiments, more specifically, the error in pose estimation may lead to an increase in the overall error of the global map. By utilizing the neighboring point information of edge feature points and plane feature points, the algorithm can more accurately estimate lines and surfaces in the global map.

[0090] Line features use the distance from a point to a line to measure the matching degree between feature points and line features in the local map, which can effectively reflect the distribution of feature points near the line. The calculation formula is as follows:

[0091]

[0092] in, represents the i-th position of the autonomous vehicle at time k+1.

[0093] Surface features use the distance from a point to a plane to evaluate the matching degree between feature points and surface features in the local map. The calculation formula is as follows:

[0094]

[0095] By summing the distances of all points to the line and to the plane, we get the overall error assessment, which is the loss function. The calculation formula is as follows:

[0096]

[0097] This loss function aims to accurately estimate the pose change of the autonomous vehicle at each moment through the optimization process, thereby constructing a series of coherent and accurate pose estimates. These estimated poses can be accurately represented by transformation matrices, each of which contains the translation and rotation information of the autonomous vehicle in space. The transformation matrix is ​​as follows:

[0098]

[0099] in, is the pose transformation matrix of the autonomous vehicle at the k+1th frame compared to the previous frame (i.e., the kth frame), t x ,t y and t z The parameters representing the translation part of the transformation matrix, θ x ,θ y and θ zParameters representing the rotation part of the transformation matrix.

[0100] For each edge feature point p e ∈E k , calculate the covariance matrix of its nearest neighbor in the global feature map (i.e., global map). When the point is on a straight line, the covariance matrix will have a maximum eigenvalue, and the direction of the normal vector of the line is determined to be the eigenvector n corresponding to this maximum eigenvalue. e , and the position of the straight line is regarded as the geometric center of the nearby points. Similarly, for each plane feature point p s ∈S k , calculate the covariance matrix of its nearest neighbor in the global map. When the point is located on a plane, the covariance matrix of its nearest neighbor has a minimum eigenvalue. Based on this, the normal vector direction of the plane is determined to be the eigenvector n corresponding to this minimum eigenvalue. s , the position of the plane is

[0101] In the loss function, the edge feature point p e Distance f to the global map line feature e The calculation formula is as follows:

[0102]

[0103] Among them, T k is the pose transformation matrix of the autonomous vehicle at the kth frame compared to the previous frame, p n is a unit vector, and its calculation formula is as follows:

[0104]

[0105] In the loss function, the plane feature point p s Distance f to global map surface features s The calculation formula is as follows:

[0106]

[0107] For each feature point in the local map, the intensity distribution index PI value of its surrounding neighboring points is calculated separately:

[0108]

[0109] Using the above equation of the intensity distribution index PI value and taking the intensity distribution index PI value as the weight value, the final pose of the autonomous vehicle can be estimated by minimizing the weighted sum of the distances from point to line and point to plane. Its expression is as follows:

[0110]

[0111] in, Indicates the PI value of the intensity distribution index of the neighboring points of the edge feature point. Represents the intensity distribution index PI value of the neighboring points of the plane feature point; Represents the final pose estimate.

[0112] The expression of the transformation matrix is ​​given above, so the translation part of the transformation matrix can be expressed as:

[0113] t=[t x t y t z ] T

[0114] The rotation part of the transformation matrix can be expressed as:

[0115]

[0116] in,

[0117]

[0118] Among them, c x 、c y and c z are the cosine values ​​of the rotation angles around the x, y, and z axes, respectively, and s x 、s y and z is the corresponding sine value.

[0119] By applying the above rotation and translation transformations, the transformation of the point cloud from the current frame to the next frame can be calculated to update its position in the global coordinate system. The update process of the point cloud position can be expressed by the following transformation relationship:

[0120]

[0121] where G is the matrix that applies the rotation and translation, is the transformation matrix from k to k+1.

[0122] A function is defined to measure the difference between the transformed point cloud data and the reference model. The expression of this function is:

[0123]

[0124] Solve for the optimization variable using Levenberg iterative optimization:

[0125]

[0126] To minimize this function, the Jacobian matrix J is used to express the local derivatives of the transformation matrix with respect to its parameters. The calculation formula of the Jacobian matrix is:

[0127]

[0128] Evaluate the loss of the transformation from the current frame to the reference frame, which can be expressed by the following partial derivatives:

[0129]

[0130] The influence of the components in the translation vector on the position of the transformed point cloud can be calculated by the following formula:

[0131]

[0132] The influence of the components in the rotation vector on the position of the transformed point cloud can be calculated by the following formula:

[0133]

[0134] The positioning method of the present application integrates the intensity information of point cloud data to optimize the residual, takes into account the spatial distribution of the point cloud, and incorporates the intensity attribute of the point cloud into the calculation, making the loss function more refined, so that the transformation matrix can be used through the optimization process of the loss function to more accurately estimate the final pose of the target object, thereby improving the positioning accuracy of the target object.

[0135] Another embodiment of the present application provides a map construction method, which may mainly include the following steps.

[0136] Step 11: The final pose estimation value obtained based on step 3 of the above embodiment By aligning the scanning frame to the local map and utilizing the spatial relationship of the point cloud, a fused feature descriptor is constructed based on the fusion of height features and intensity features to achieve effective modeling and representation of the surrounding environment, and to build a lidar SLAM model based on intensity information optimization loop.

[0137] In some embodiments, specifically, the point cloud intensity mean is incorporated into the feature description of the unit box model, the dimension of the feature descriptor is increased, the discriminability of the feature points is improved, and at the same time, the relative distance to the current frame feature descriptor is minimized, the frame with the highest similarity to the current scan frame in the historical data is accurately identified, the cumulative error introduced by the sensor position and posture deviation is eliminated, and a lidar SLAM model based on the intensity information optimization loop is constructed.

[0138] More specifically, the point cloud in a single scan frame is divided into N equal parts in the radial direction in polar coordinates. r Rings, the width of each ring is:

[0139]

[0140] Among them, L max is the maximum distance of the laser point.

[0141] Then, the point cloud in a single scan frame is divided into N equal parts according to the azimuth in polar coordinates. s sectors, and the arc value of each sector is as follows:

[0142]

[0143] Based on this, the point cloud can be divided into N s *N r The division results are as follows: Figure 2 As shown, the calculation formula for the set P of all fan ring unit boxes is as follows:

[0144]

[0145] Among them, P ij is the set of points in the sector ring unit box composed of the ith ring and the ith sector, i∈[|1,N s |],j∈[|1,N r |].

[0146] For the height feature in the fan ring unit box, by traversing all the points in the fan ring unit box, the point with the maximum height value is selected to approximately replace all other points in the fan ring unit box, and the height value of this point is used as the height feature value of the fan ring unit box, such as Figure 3 The calculation formula for solving the maximum height value of the point inside the fan ring unit box is as follows:

[0147]

[0148] Wherein, z(p) is the height value of point p inside the fan ring unit box.

[0149] The method of relying solely on the height value of the point cloud as a similarity metric for the scan context fails to fully utilize the potential value of the intensity information. In fact, the intensity information of the point cloud can also play an important role in similarity evaluation. In order to make more comprehensive use of the point cloud data, the intensity features in the fan ring unit box are processed as follows.

[0150] First, identify and remove points with intensity values ​​less than 10 in the fan ring unit box (the normalized intensity values ​​are stored in the point cloud data of PointXYZRGB type, and points with intensity values ​​less than 10 are removed from this point cloud data), because these points are usually regarded as invalid noise points, which not only have no positive effect on subsequent processing, but may interfere with the eigenvalue calculation of the fan ring unit box.

[0151]

[0152] Among them, I(p k ) is the point p inside the fan ring unit box k The intensity value of .

[0153] Next, the nearest points are searched for the remaining points, and the five nearest points are taken one by one to solve the intensity average. Finally, the highest value is selected from these intensity averages as the intensity feature value of the fan ring unit box. This method not only considers the local intensity distribution of the points in the fan ring unit box, but also highlights the feature points with higher intensity, so that it can more accurately reflect the intensity characteristics of the fan ring unit box, such as Figure 4 The formula for solving the mean intensity is as follows:

[0154]

[0155] Among them, N i Represents point p k The number of nearby neighbor points found.

[0156] The calculation formula of the strength characteristic value of the fan ring unit box is as follows:

[0157]

[0158] In order to further filter out invalid matches or false loops, intensity information is introduced as an additional similarity metric. By combining intensity information with height values, the similarity between scan contexts can be more comprehensively evaluated, thereby improving the accuracy and robustness of loop detection. This innovative method not only enriches the feature representation of loop detection, but also provides a more accurate environment model for the mapping system, thereby providing further optimization space for the performance and reliability of the LiDAR SLAM model based on intensity information to optimize loops.

[0159] Specifically, the maximum height value and the mean intensity of the points in the fan ring unit box are weighted and summed to serve as the eigenvalue of the fan ring unit box. This weighted sum method fully considers the information of the two dimensions of height and intensity, so that the eigenvalue more comprehensively reflects the characteristics of the fan ring unit box. The calculation formula is as follows:

[0160]

[0161] Where z(p) is the Z coordinate value of point p in the fan ring unit box, ω TRepresents the weight value of the sector ring unit box intensity mean feature index. These rich fusion feature descriptors enable the algorithm model to perform matching operations more accurately in complex environments, effectively close loops, improve the discrimination of feature points, and enhance the SLAM system's ability to recognize the environment, thereby reducing error accumulation.

[0162] The final pass size is N r *N s The matrix stores the scanning context information T, and its calculation formula is as follows:

[0163]

[0164] Based on the characteristic value of the fan ring unit box, the signal strength of each scanning point is extracted and sorted to obtain an N r *N s A matrix of size T. Each element a of the matrix ij Represents the signal strength at the position of row i and column j. This matrix structures the scanning context information, making subsequent analysis and processing more convenient.

[0165] During the driving process of the autonomous vehicle, once the scanning context information is generated by the point cloud collected by the current scanning frame, it needs to be compared with the previously stored scanning context information. In order to speed up this comparison process, first traverse the number of feature points in different circles of the current scanning frame, and generate a feature distribution vector based on the number of feature points. The feature distribution vector can intuitively reflect the feature distribution of the current scanning frame in different circles and sectors:

[0166]

[0167]

[0168] Among them, ||r i ||0 means the radius in the current scan frame is r i The number of non-empty sector ring unit boxes in the ring.

[0169] By preliminarily comparing the components in the feature distribution vector, we can quickly filter out those historical frames that are significantly different from the current frame, thereby greatly reducing the workload of subsequent detailed comparisons. Next, the scanning context information of the current frame is compared in detail with the preliminarily filtered historical frame information to form a scanning context pair. In order to more accurately evaluate the similarity between the two contexts, a distance calculation formula for a scanning context pair is defined:

[0170]

[0171] Among them, the column vector is calculated by cosine distance, Ns Also indicates the number of columns (N r *N s The matrix stores the scanning context information T), T q is the feature descriptor of the current frame, T c is the feature descriptor of the historical frame. It is T q The jth column of It is T c The jth column of and are their respective norms.

[0172] By calculating the cosine distance between the feature descriptor of the current frame and the feature descriptor of the historical frame, a quantitative index can be obtained to evaluate the similarity between the two. This similarity measurement method takes into account both the quantity distribution of features and the relative relationship between them, thereby improving the accuracy and robustness of loop detection.

[0173] Step 12: Input the original point cloud data obtained when the target moves into the constructed laser radar SLAM model based on the intensity information optimization loop, and output the final point cloud map.

[0174] The positioning method and map construction method based on the intensity information optimization loop of the embodiment of the present application have achieved the minimum mapping error with the true value of the trajectory in the sequence of multiple KITTI datasets, such as Figure 5 In addition, the mean absolute trajectory errors of these sequences after mapping are basically the lowest, as shown in the IFSC-LOAM model in Table 1 (a laser radar SLAM model based on intensity information optimization loop constructed in the embodiment of the present application).

[0175] Table 1. Comparison of absolute trajectory error results of lidar odometer system on KITTI dataset

[0176]

[0177] It is worth noting that in order to verify the superiority of the method proposed in this application, the embodiment of this application selects the experimental data as the open source KITTI data set for verification. The KITTI data set is collected by a vehicle equipped with a Velodyne 64-line 3D laser radar, covering environmental information under various complex road conditions such as cities, rural areas and highways, and can comprehensively examine the generalization performance of the algorithm. This application uses a laptop equipped with an Intel Core i7-12700 H CPU @ 2.30 GHz and 16 GB of memory, running the Ubuntu 20.04 operating system. This technical achievement fully demonstrates the effectiveness of obtaining more accurate pose information, improving loop detection by introducing intensity information, and the advantages of the laser radar SLAM algorithm model based on intensity information optimization loop in improving mapping accuracy and robustness. This technical achievement has certain promotion value in the field of automatic navigation and positioning technology.

[0178] The flowchart and / or block diagram in the accompanying drawings illustrate the possible architecture, functions and operations of the system, method or computer program product according to various embodiments of the present application. In this regard, each box in the flowchart and / or block diagram can represent a part of a module, program segment or code, and a part of the above-mentioned module, program segment or code includes one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from that marked in the accompanying drawings. It should also be noted that each box in the block diagram or flowchart, and the combination of the boxes in the block diagram or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0179] Those skilled in the art will appreciate that the technical features described in the various embodiments and / or claims of the present application may be combined and / or combined in a variety of ways, even if such combinations and / or combinations are not explicitly described in the present application. In particular, without departing from the spirit and teachings of the present application, the technical features described in the various embodiments and / or claims of the present application may be combined and / or combined in a variety of ways, and all of these combinations and / or combinations fall within the scope of the present application.

[0180] Although the present application has been shown and described with reference to specific exemplary embodiments of the present application, it should be understood by those skilled in the art that various changes in form and details may be made to the present application without departing from the spirit and scope of the present application as defined by the appended claims and their equivalents. Therefore, the scope of the present application should not be limited to the above-mentioned embodiments, but should be determined not only by the appended claims, but also by the equivalents of the appended claims.

Claims

1. A positioning method, characterized in that: include: Obtain the original point cloud data of the target object when it moves and extract feature indicators; The persistent features and intensity features of the original point cloud data are integrated to construct an intensity distribution index function, and a preset effective feature point filtering threshold is established in conjunction to remove dynamic points from the feature point set; The function value of the intensity distribution index function is used as a weight value, and the final pose estimation value of the target object is obtained by minimizing the weighted sum of the distances from the edge feature points to the global map line features and the distances from the plane feature points to the global map surface features.

2. The positioning method according to claim 1, characterized in that: The method of fusing the persistent features and intensity features of the original point cloud data, constructing an intensity distribution index function, and simultaneously setting a preset effective feature point filtering threshold to remove dynamic points from the feature point set includes: The expression of the intensity distribution index function constructed includes: in, represents the intensity distribution index value, represents a feature point in the local map M in the k0th scanning frame, k represents the number of the scanning frame; ω P and ω I They represent the weight values ​​of the persistence characteristic index and the intensity distribution index respectively; represents the persistent feature index value, γ∈[0,1] and τ represent parameters used to measure the influence of the time interval on the persistent feature index value, Indicates whether there is a feature point in the τth frame that can be compared with Matching evaluation value; (σ i -θ i ) represents the strength characteristic index value, σ i Represents the intensity distribution value of the nearest neighbor of the feature point, θ i Indicates the intensity threshold of valid feature points; like Then the feature point at this time Identify as a valid feature point and retain it; if Then the feature point at this time Identify as invalid feature points and remove them; θ pi Indicates the effective feature point filtering threshold.

3. The positioning method according to claim 2, characterized in that: The expression of the final pose estimation value of the target object includes: in, Represents the final pose estimation value of the target object; Indicates the intensity distribution index value of the neighboring points of the edge feature point; Represents the intensity distribution index value of the neighboring points of the plane feature point; f e (p e ) represents the edge feature point p e Distance to global map line feature; f s (p s ) represents the plane feature point p s The distance to the global map surface feature.

4. The positioning method according to claim 3, characterized in that: The edge feature point p e Expressions for distances to global map line features include: Among them, T k represents the pose transformation matrix of the target object at the kth frame compared with the previous frame; n e and They represent the normal vector and position of the line where the edge feature point is located when the covariance matrix of the nearest neighbor point in the global map takes the maximum eigenvalue; p n is a unit vector; The plane feature point p s Expressions for distances to global map surface features include: Among them, n s and They respectively represent the normal vector and position of the plane where the plane feature point is located when the covariance matrix of the nearest neighbor point in the global map takes the minimum eigenvalue.

5. The positioning method according to claim 2, characterized in that: The expression of the matching evaluation value includes: Among them, E k represents the set of edge feature points of the kth frame; S k N represents the set of plane feature points of the kth frame; k (p k ) indicates p k The set of points corresponding to the local map M; when When , it means that there is a feature point in the kth frame that can be compared with Matching, feature points that can be continuously observed in multiple consecutive frames are considered as persistent feature points; The expression of the intensity distribution value of the nearest neighbor point of the feature point includes: Among them, N i Representing feature points The number of neighboring points found by nearest neighbor search in the local map M; η i Represents the intensity value of the corresponding point in the point cloud.

6. A map construction method, characterized in that: include: Based on the final pose estimation value obtained in any one of claims 1 to 5, by registering the scanned frame to the local map, utilizing the spatial relationship of the point cloud, based on the fusion of height features and intensity features, effective modeling and representation of the surrounding environment is achieved, and a lidar SLAM model based on intensity information optimization loop is constructed; The original point cloud data obtained when the target object moves is input into the laser radar SLAM model based on the intensity information optimization loop, and the final point cloud map is output.

7. The map construction method according to claim 6, characterized in that: The method of registering the scanned frame to the local map, utilizing the spatial relationship of the point cloud, and fusing the height feature and the intensity feature to achieve effective modeling and representation of the surrounding environment includes: Perform weighted summation on the representative height value and the representative intensity mean value of the points in the fan ring unit box to obtain the characteristic value of the fan ring unit box; Based on the characteristic value of the fan ring unit box, the signal strength of each scanning point is extracted and sorted to obtain N r *N s The size of the matrix T representing the scan context information; where N r and N s Respectively represent the number of equally divided rings in the radial direction and the number of equally divided sectors in the circumferential direction of the point cloud in a single scanning frame in polar coordinates; each element a of the matrix T ij represents the signal strength at the position of row i and column j; Compare the scanning context information of the current frame with the scanning context information of the historical frames to construct a scanning context pair; Obtaining a distance value of the scanning context pair based on a cosine distance between a current frame feature descriptor and a historical frame feature descriptor; According to the distance value of the scanning context pair, the similarity between the scanning context information of the current frame and the scanning context information of the historical frame is evaluated to obtain a corresponding evaluation result.

8. The map construction method according to claim 7, characterized in that: The expression of the characteristic value of the fan ring unit box includes: f(P ij )=Φ(P ij )+ω T I′ max (p); Among them, P ij represents the set of points in the sector ring unit box composed of the i-th ring and the j-th sector; f(P ij ) represents the characteristic value of the fan ring unit box; Φ(P ij ) represents the fan ring unit box P ij The maximum height value of the inner point is the representative height value; I' max (p) represents the maximum intensity mean of a preset number of nearest neighbor points, which is the representative intensity mean; ω T I′ max The weight value of (p).

9. The map construction method according to claim 8, characterized in that: include: Eliminate points in the fan ring unit box whose intensity values ​​are lower than the preset intensity threshold; Performing a neighbor point search for the remaining points in the fan ring unit box, taking the preset number of nearest neighbor points one by one and solving their corresponding intensity means respectively; The largest one among these intensity means is selected as the intensity characteristic value of the sector ring unit box.

10. The map construction method according to claim 9, characterized in that: include: Traverse the number of feature points in different circles of the current scan frame, and generate a feature distribution vector based on the number of feature points; The expression of the characteristic distribution vector is: Among them, ||r i ||0 means the radius in the current scan frame is r i The number of non-empty fan ring unit boxes in the ring; Compare the components in the feature distribution vector to select historical frames that have a difference with the current scan frame exceeding a preset threshold; Compare the scanning context information of the current frame with the scanning context information of the filtered historical frames to construct a scanning context pair; Based on the cosine distance between the current frame feature descriptor and the filtered historical frame feature descriptor, the distance value of the scanning context pair is obtained, and its expression includes: Among them, T q represents the feature descriptor of the current frame, T c is the feature descriptor of the filtered historical frame; It is T q The jth column of It is T c The jth column of and are the corresponding norms respectively; d(T q ,T c ) represents the distance value of the scanning context pair.

Images