LiDAR point-line-surface feature uncertainty adaptive modeling method and device

Abstract

The invention provides a self-adaptive modeling method and device for uncertainty of LiDAR (Light Detection and Ranging) point-line-surface features. According to the method, a maintainable parameterized local sub-graph is constructed through parameterized extraction and tracking of plane / line features, modeling is carried out on uncertainty of point clouds and parameterized plane / line features in the local sub-graph, and therefore observation covariance is determined in a self-adaptive mode. According to the method, the LiDAR variance is reasonably modeled, so that better fusion with other sensors can be realized, the GNSS / INS / LiDAR positioning performance in various complex urban environments can be effectively improved, and the inconsistency of the LIO (LiDAR Inertial Orientation, LIO) state covariance is improved.

Description

Technical Field

[0001] The present invention belongs to the field of GNSS / INS / LiDAR fusion positioning, and in particular relates to a LiDAR point, line, and surface feature uncertainty adaptive modeling method and device. Background Art

[0002] With the development of artificial intelligence, robotics, and the Internet of Things, society has entered the intelligent era. Various autonomous mobile robots, including drones (airborne), autonomous driving (car-borne), and unmanned ships (ship-borne), have developed rapidly to meet the needs of various complex tasks and liberate human labor. In order for robots to complete tasks autonomously, their core key technologies include navigation and positioning, environmental perception, planning and decision-making, and control, among which navigation and positioning is the basic module. At present, most robots are equipped with various sensors to simulate humans, which can be divided into two categories: positioning and attitude sensors, such as GNSS (Global Navigation Satellite System, GNSS), IMU (Inertial Measurement Unit, IMU), odometer, etc.; environmental perception sensors such as Camera, LiDAR (Light Detection And Ranging, LiDAR), millimeter wave radar, etc. These sensors are developing in the direction of fusion, and multi-source information fusion has become a research hotspot in the field of robotics. LiDAR (Light Detection And Ranging, LiDAR) can detect rich geometric information in the environment and is increasingly widely used in navigation and positioning. Due to the complementary characteristics of GNSS, INS, and LiDAR, the fusion positioning of the three has been widely studied. In multi-source fusion positioning, uncertainty (covariance) modeling of each sensor is crucial. GNSS and IMU covariance modeling is mature, while LiDAR covariance modeling is not. Specifically, the original point cloud covariance, local map covariance, and observation covariance are not accurately modeled, resulting in unsatisfactory GNSS / INS / LiDAR fusion.

[0003] In summary, the current GNSS / INS / LiDAR fusion positioning mainly has the following problems: ① LiDAR observations are mostly used for matching single-frame feature points to local maps, and it is impossible to form common-view constraints for LiDAR multi-frame data; ② There is a lack of research on the reasonable determination of LiDAR point, line and surface feature variance, and it is difficult to perform variance modeling on LiDAR observations, which easily leads to inconsistent LiDAR variance. Summary of the invention

[0004] In view of the above problems, the present invention provides a LiDAR point, line and surface feature uncertainty adaptive modeling method and device, which constructs a maintainable parameterized local subgraph by parametrically extracting and tracking line and surface features, and models the uncertainty of point clouds and parameterized plane / line features in the local subgraph, thereby adaptively determining the observation covariance, effectively improving the positioning performance of GNSS / INS / LiDAR in various complex urban environments and improving the LIO variance consistency.

[0005] According to one aspect of the present invention, a LiDAR point, line and surface feature uncertainty adaptive modeling method is provided, comprising the following steps:

[0006] Get LiDAR point cloud data;

[0007] Extract line and surface feature points from non-ground points based on the acquired LiDAR point cloud data;

[0008] Based on the extracted line and surface feature points, extract, merge and track line and surface feature landmarks;

[0009] Based on the point cloud on the obtained line and surface feature landmark, point cloud uncertainty modeling is performed, and based on the radar points with uncertainty after point cloud uncertainty modeling, initialization line and surface feature uncertainty modeling is performed to obtain initialization line and surface features with uncertainty;

[0010] Based on the point cloud on the line and surface feature landmarks obtained, the key point cloud on the line and surface feature landmarks is screened according to the line and surface geometric characteristics and the minimum representation method;

[0011] Based on the initialized line and surface features with uncertainty and the screened key point cloud, point-line and point-surface distance constraints are constructed, and the observation noise is modeled based on variance propagation.

[0012] As a further technical solution, based on the acquired LiDAR point cloud data, line and surface feature points in non-ground points are extracted, including:

[0013] Based on the acquired LiDAR point cloud data, the ground points are extracted in combination with the Ground-Detection algorithm;

[0014] Based on the extracted ground points, non-ground points of the LiDAR point cloud data are obtained;

[0015] Based on the obtained non-ground points, curvature threshold segmentation is performed to extract line and surface feature points from the non-ground points.

[0016] As a further technical solution, based on the extracted line and surface feature points, line and surface feature landmarks are extracted, merged and tracked, including:

[0017] Adopt Lin e / Plan e -Detection algorithm extracts line / surface feature landmarks respectively;

[0018] Based on the extracted line / surface feature landmarks, line and surface features are merged;

[0019] Based on the line-surface feature merging results, data association is performed to find the correspondence between the line / surface features of multiple lidar frames in the sliding window to achieve line-surface feature tracking.

[0020] As a further technical solution, based on the point cloud obtained on the line and surface feature landmark, point cloud uncertainty modeling is performed, and based on the radar points with uncertainty after the point cloud uncertainty modeling, initialization line and surface feature uncertainty modeling is performed to obtain the initialization line and surface features with uncertainty, including:

[0021] According to the range uncertainty and azimuth uncertainty of the lidar point in the local lidar frame, the point cloud uncertainty modeling is performed to obtain the lidar point with uncertainty

[0022] By having covariance The estimated pose and The laser radar point Project it into the fixed anchor coordinate system w, and obtain the uncertainty of the projection point through variance propagation,

[0023]

[0024] in, are the i-th point in the LiDAR coordinate system, the w coordinate system, and the ECEF coordinate system respectively; is the rotation at the kth moment uncertainty, It's location uncertainty; further use of the projection point Fit the line and surface features to obtain the line segment parameters LF(l, m) and plane parameters PF(n, q c ) and its uncertainty.

[0025] As a further technical solution, based on the point cloud obtained on the line and surface feature landmarks, according to the line and surface geometric characteristics and the minimum representation method, the key point cloud on the line and surface feature landmarks is screened, including:

[0026] The SVD algorithm is used to calculate the eigenvalue λ of the covariance matrix A of the surface feature point cloud. The plane PF is divided into four sub-planes PF according to the eigenvectors u1 and u2 corresponding to the largest and second largest eigenvalues ​​λ1 and λ2. 1 , PF 2, PF 3 , PF 4 , average the point cloud on each sub-plane and calculate the center point of each sub-plane in turn and the center of the entire plane q c Together they form a key point cloud on the surface feature landmarks;

[0027] Compute the line segment center point q based on the point cloud on the line feature c , calculate the point on the line With the center point q c The maximum distance D, according to q c , D and the direction vector l of the line segment, calculate the upper and lower endpoints eq1 and eq2 of the line segment to form the key point cloud on the line feature landmark.

[0028] As a further technical solution, point-line and point-surface distance constraints are constructed, and the observation noise is modeled based on variance propagation, including:

[0029] Point-line constraints and variance modeling:

[0030] The distance constraint from point to line is as follows

[0031]

[0032] To project the line feature points to the w system:

[0033]

[0034] Where × represents the cross product, vector ζ=(l w , m w ) T , l is the direction vector of the line, Assumptions and The uncertainty of the projected point and plane is used to determine the uncertainty of the distance observation from the point to the plane;

[0035]

[0036] Point-surface constraints and variance modeling:

[0037] For plane observation, construct the point-surface observation equation,

[0038]

[0039] is the projection point projected to the anchor coordinate system w; is the Jacobian matrix about LiDAR pose; HΠ is the Jacobian matrix about surface feature points. Similarly, the uncertainty of point-surface observation is obtained

[0040]

[0041] According to one aspect of the present invention, there is provided a LiDAR point, line and surface feature uncertainty adaptive modeling device, comprising:

[0042] The first main module is used to obtain laser radar point cloud data;

[0043] The second main module is used to extract line and surface feature points from non-ground points based on the acquired laser radar point cloud data;

[0044] The third main module is used to extract, merge and track line and surface feature landmarks based on the extracted line and surface feature points;

[0045] The fourth main module is used to perform point cloud uncertainty modeling based on the point cloud on the line and surface feature landmarks obtained, and to perform initialization line and surface feature uncertainty modeling based on the radar points with uncertainty after the point cloud uncertainty modeling, so as to obtain the initialization line and surface features with uncertainty;

[0046] The fifth main module is used to screen and obtain key point clouds on the line-surface feature landmarks based on the point clouds on the line-surface feature landmarks obtained according to the line-surface geometric characteristics and the minimum representation method;

[0047] The sixth main module is used to construct point-line and point-surface distance constraints based on the initialized line and surface features with uncertainty and the screened key point cloud, and to model the observation noise based on variance propagation.

[0048] According to one aspect of the present specification, there is provided an electronic device, comprising: at least one processor, at least one memory and a communication interface; wherein the processor, memory and communication interface communicate with each other; the memory stores program instructions executed by the processor, and the processor calls the program instructions to execute the described method.

[0049] According to one aspect of the present specification, a non-transitory computer-readable storage medium is provided, wherein the non-transitory computer-readable storage medium stores computer instructions, wherein the computer instructions enable the computer to execute the method described above.

[0050] Compared with the prior art, the present invention has the following beneficial effects:

[0051] The present invention constructs a maintainable parameterized local subgraph by parametrically extracting and tracking line and surface features, and models the uncertainty of point clouds and parameterized plane / line features in the local subgraph, thereby adaptively determining the observation covariance, effectively improving the positioning performance of GNSS / INS / LiDAR in various complex urban environments and improving the LIO variance consistency. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 Flow chart of the LiDAR point, line and surface uncertainty adaptive modeling method provided in an embodiment of the present invention.

[0053] Figure 2 A flow chart for classifying ground points and line and surface feature points provided in an embodiment of the present invention.

[0054] Figure 3 This is a flow chart of Line / Plane-Detection feature extraction provided by an embodiment of the present invention.

[0055] Figure 4 This is a flow chart of Line / Plane-Detection line and surface feature merging provided in an embodiment of the present invention.

[0056] Figure 5 This is a Line / Plane-Detection line and plane feature tracking flow chart provided by an embodiment of the present invention.

[0057] Figure 6 A flow chart of line and surface feature uncertainty modeling provided in an embodiment of the present invention.

[0058] Figure 7 A flowchart for screening key point clouds of line and surface features provided in an embodiment of the present invention.

[0059] Figure 8 A schematic diagram of the structure of a LiDAR point, line, and surface uncertainty adaptive modeling device provided in an embodiment of the present invention.

[0060] Fig. 9 A schematic diagram of the structure of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0061] The present invention is a LiDAR point, line and surface feature uncertainty adaptive modeling method, such as Figure 1 As shown. The present invention constructs a maintainable parametric local subgraph by parametrically extracting, merging and tracking plane / line features, and models the uncertainty of point clouds and parametric plane / line features in the local subgraph, then screens the key point clouds on the line and surface feature landmarks, and finally uses the key point clouds to construct point-line / surface distance constraints, while improving the computational efficiency and adaptively determining the observation covariance. By reasonably modeling the LiDAR variance, it is possible to achieve better integration with other sensors, effectively improve the GNSS / INS / LiDAR positioning performance in various complex urban environments, and improve the inconsistency of the LIO state covariance.

[0062] The following will be combined Figure 1The key steps and implementation schemes of the present invention are described in detail.

[0063] 1. Point cloud preprocessing

[0064] The time point of each laser point emission of the commonly used mechanical laser radar is different, and the laser point cloud within a frame is not in the same local coordinate system. In addition, due to the influence of external environmental factors such as atmospheric pressure, temperature, humidity, and target entity reflection characteristics, the collected laser point cloud has a certain error. By performing pre-processing operations such as filtering, denoising, and segmentation on the point cloud data obtained by the input laser radar, new and more accurate point cloud data can be obtained.

[0065] 2. Line and surface feature extraction, merging and tracking

[0066] Since the 3D LiDAR sensor can provide a large number of 3D point clouds, different scanning points usually represent different physical locations. Considering the difficulty in matching different scanning points and the real-time requirements of the system, it is impossible to align and track all points. In practical applications, it is not necessary to process each point separately. The original point cloud is used to extract line, surface and other feature information, which can effectively improve the effect of feature tracking and matching while saving computing resources. Therefore, the present invention has developed a line and surface feature extraction, merging and tracking algorithm with a multiple inspection mechanism to effectively track line and surface features between multiple LiDAR frames in a sliding window. The specific implementation steps are as follows:

[0067] Step 1: In the actual road environment, a frame of point cloud usually contains a large number of ground point clouds. Using an efficient algorithm to pre-extract and fit the ground point cloud can not only effectively estimate the ground parameters, but also improve the efficiency of line and surface feature point extraction and tracking algorithms. The ground-detection algorithm is used to extract ground points from the pre-processed point cloud data, such as Figure 2 As shown. The point cloud is divided into ground points and non-ground points, and the curvature of non-ground points is further calculated, and threshold segmentation is performed to extract line and surface feature points from non-ground points. The specific steps include the following:

[0068] (1) Extracting ground plane point cloud: one frame of point cloud The point with the lowest height is most likely to belong to the ground plane, so if the k-th point cloud height z(pk) is less than a certain distance from the LiDAR height h, the point is added to the initial estimated ground point set. Among them, p k represents the kth point cloud, l represents the radar coordinate system, t represents the time t, T seed Indicates the ground point height threshold,

[0069]

[0070] Calculate seed points The covariance matrix of the ground point is decomposed into eigenvalues ​​to obtain the minimum eigenvalue λ1 and the corresponding eigenvector u1 (normal vector). If the distance is less than a certain threshold τ g Then the point is considered to be a ground plane point and added to the ground point set After iterating the plane fitting 3-5 times, the final ground points and ground parameters can be obtained as shown below, where n represents the ground normal vector and T represents the transpose. The average value of all points in the ground point set,

[0071]

[0072] (2) Curvature threshold segmentation algorithm: Calculate the curvature of the non-ground point cloud, where points with low curvature and points with high curvature are classified as plane feature points and line feature points, respectively, denoted by P and L;

[0073]

[0074] S is the set of five points before and after the non-ground point on the same scan line, |S| = 10; r i , r j express The depth corresponding to the point, represents the coordinates of the i-th point in the Lidar coordinate system at time k, Represents the coordinates of the jth point in the Lidar coordinate system at time k. This formula determines whether the i-th point belongs to a plane feature point or a line feature point. If the curvature c of the point is greater than the threshold, it is attributed to the edge feature point set; points with a c value less than the threshold are attributed to the surface feature point set.

[0075] Step 2: In actual indoor and outdoor environments, most of the objects scanned by the laser radar can be represented by two basic geometric structures: lines and surfaces. These "lines" and "surfaces" are generally evenly distributed in various positions in the actual three-dimensional environment. At the same time, the directions of the lines and the normal vectors of the surfaces are relatively uniform in all directions in the three-dimensional environment. These characteristics ensure that the line and surface feature points can basically represent the geometric features of the environment. Subsequent line and surface feature processing using the extracted line and surface feature points can balance the algorithm between accuracy and efficiency. Therefore, the present invention uses Lin for the line and surface feature points obtained in step 1. e / Plan e -Detection algorithm is used to extract line and surface feature parameters, such as Figure 3 The specific steps include:

[0076] (1) Line feature parameterization. For the point p0∈L belonging to the i-th scan line, first determine the closest point p1∈L on the i+1-th scan line. Fit a straight line using p0 and p1. Then find the point p2∈L on the (i+2)-th scan line that is closest to p1. If the distance between p2 and the scan line is less than a threshold (0.1 mm), fit a new scan line using p0, p1, and p2. This process is repeated until no more points can be added. When fitting a straight line, we first calculate the center point q of the line segment. c (x0, y0, z0) and covariance A are as follows, where N represents the number of points on the new scan line obtained by the previous fitting. j ,y j 、z j represents the coordinates of the jth point,

[0077]

[0078] Among them, (x j ,y j , z j ) T =p i -q c , p i ∈L, i∈{1, 2, ..., N}. We use Planck coordinates LF(l, m) to parameterize the line. Where l is the direction of the line, ||l||=1, m=q c ×l, its size is the distance from the origin to the straight line, and its direction is perpendicular to the plane determined by the origin and the straight line.

[0079] (2) Surface feature parameterization. For each point in the surface feature point set P on the i-th scan line, use the KNN algorithm to search for its k nearest neighboring points outside the i-th scan line. Then use the SVD algorithm to perform eigenvalue decomposition on the covariance matrix to obtain the plane normal vector n (i.e., the eigenvector corresponding to the minimum eigenvalue of A) and the center point q c The plane can be described as PF(n,q c ). Calculate the sum of the distances from all points to the plane and remove points that are greater than the threshold (set to 0.2m).

[0080] Step 3: The line and surface feature information extracted in step 2 may be located on the same line / surface. In order to avoid repeated use of observation information and improve calculation efficiency, the present invention merges the line and surface features extracted in step 2, such as Figure 4 For each selected PF i , use KD tree to search its adjacent PF j . Construct the linear residual and plane residual to determine whether the two lines / areas can be considered as the same line / area.

[0081]

[0082] First calculate the residual first normal vector distance n i -n j If it is less than the empirical threshold, we further focus on the second term of the residual T represents the i-th plane normal vector n i If both checks pass, the two planes are considered to correspond to the same plane. In this case, a new plane can be refitted using the points on the two planes. The process of merging line features is similar. If the two If both checks pass, it is considered that the two lines correspond to the same line.

[0083] Step 4: After extracting and merging features in step 3, the present invention performs data association on line and surface features to find the corresponding relationship between multi-frame point cloud features in the sliding window, such as Figure 5 As shown. Using IMU to calculate the relative posture between time k-1 and k, the plane with index i in the kth frame can be projected onto the k-1th frame, and its nearest plane feature with index j can be found. By calculating the residual To determine whether two planes represent the same plane.

[0084]

[0085] is the pose of the k-1th frame, is the parameter of the jth plane in the k-1th frame, is the normal vector of plane j in the Lidar coordinate system at time k-1. is the coordinate of the center point of plane j in the Lidar coordinate system at time k-1. It is the rotation matrix from the Lidar coordinate system to the ECEF system at time k-1. is the Lidar coordinate in the ECEF system at time k-1. is the coordinate of the center point of plane j at time k obtained by projection transformation at time k-1. If the residual is greater than the empirical threshold (n is 0.2rad, d is 0.3m), we will refuse to associate plane feature i with plane feature j. Referring to plane tracking, the residual of the line It can be defined as follows:

[0086]

[0087] The physical meaning of the first item is to convert the straight line direction vector of the kth frame to the k-1th frame and obtain the difference with the direction vector of the k-1th frame. The second item is to project the average point of the kth frame to the k-1th frame and calculate the distance from the k-1th frame to the straight line.

[0088] 3. Line and surface initialization and uncertainty modeling

[0089] To Lin e / Plan e -Detection extracts and merges the point clouds on the line and surface feature landmarks after tracking to perform uncertainty modeling, projects the point clouds on the common view landmarks to the initialization coordinate system, re-fits the parameters, and performs uncertainty modeling on the initialization features through variance propagation, and finally obtains the initialization line and surface features with uncertainty in the initialization coordinate system, such as Figure 6 As shown. Specifically including point cloud, initialization line and surface feature uncertainty modeling:

[0090] Step 1: Point cloud uncertainty modeling. The uncertainty of a lidar point in the local lidar frame consists of two parts, namely, range uncertainty and azimuth uncertainty. Let ω i is the measured direction, ω i Azimuth noise on the tangent plane, d i is the depth measurement, is the ranging noise.

[0091]

[0092] N(ω i )=[N1 N1]∈R 3×2 ω i Orthogonal basis vectors on orthogonal planes, R represents a matrix set, s represents a surface, Indicates the direction ω i The variance of Indicates depth d i The variance of the measurement point p i The noise δp i and its covariance for:

[0093]

[0094] in Yes i The orthogonal basis of the tangent plane, represents the cross product, i.e. ω i The antisymmetric matrix of .

[0095] Step 2: Initialize the uncertainty modeling of line and surface features. The estimated pose and The laser radar point Project it into a fixed anchor coordinate system w and obtain the uncertainty of the projection point through variance propagation.

[0096]

[0097] in, are the i-th point in the LiDAR coordinate system, the w coordinate system, and the ECEF coordinate system respectively; is the rotation at the kth moment uncertainty, It's location The uncertainty of . Further use of the projection point Fit the line and surface features to obtain the line segment parameters LF(l, m) and plane parameters PF(n, q c ) and its uncertainty.

[0098] Denote the line segment parameter LF(l,m) as p i The function is as follows:

[0099] (l, m) T =f(p1, p2, ..., p N )

[0100]

[0101] in Assume that the eigenvector matrix of A is U, and the eigenvalues ​​are λ1, λ2, λ3 from large to small, and the corresponding eigenvectors are u1, u2, u3. The partial derivatives of l and m for each point can be calculated for the calculation of the Jacobi matrix.

[0102]

[0103] For a plane we define the plane parameter PF(n,q c ) is p i The function is as follows:

[0104] (n,q c ) T =g(p1, p2, ..., p N )

[0105]

[0106] in g represents the point p on the plane i With the plane parameters PF(n,q c ) is a function of the relationship between λ3 and λ kThey represent the third and kth eigenvalues ​​of the plane point covariance matrix A after sorting from large to small, and N represents the number of points on the plane. Similarly,

[0107]

[0108] 4. Line and surface feature key point cloud screening

[0109] When constructing point-plane / point-line distance observations, in theory, three points can construct plane constraints, and two points can construct line constraints. In practice, it is not necessary to use all point clouds on the plane and the line to construct observation constraints. Using all point cloud observations will not only reduce computational efficiency, but also lead to overly optimistic state covariance estimates. Therefore, the present invention screens the point clouds on the line-plane feature landmarks extracted and merged after tracking by Line / Plane-Detection, and screens the key point clouds on the line-plane feature landmarks based on the line-plane geometric characteristics and the minimum representation method to achieve point cloud downsampling, such as Figure 7 shown.

[0110] Step 1: Downsampling of the surface feature point cloud. First, the SVD algorithm is used to calculate the eigenvalue λ of the surface feature point cloud covariance matrix A. Then, the plane PF is divided into four sub-planes PF according to the eigenvectors u1 and u2 corresponding to the largest and second largest eigenvalues ​​λ1 and λ2. 1 , PF 2 , PF 3 , PF 4 Finally, the point cloud on each sub-plane is averaged and the center point of each sub-plane is calculated in turn. and the center of the entire plane q c Together they form the key point cloud on the surface feature landmarks.

[0111] Step 2: Downsampling of line feature point cloud. According to the geometric characteristics of the line, a line is uniquely determined by at least two points. Therefore, we first calculate the line segment center point q based on the point cloud on the line feature. c , then calculate the points on the line With the center point q c The maximum distance D is as follows:

[0112]

[0113] Finally, according to q c , D and the direction vector l of the line segment, calculate the upper and lower endpoints eq1 and eq2 of the line segment to form the key point cloud on the line feature landmark.

[0114] eq1=q c +D*l

[0115] eq2=q c -D*l

[0116] 5. Point, Line / Surface Observation Noise Modeling

[0117] According to the point cloud on the line and surface feature landmarks screened in the fourth step and the initialized line and surface features obtained in the third step, point-line and point-surface distance constraints are constructed, and the observation noise is modeled based on variance propagation.

[0118] Step 1: Point-line constraints and variance modeling. The distance constraint from point to line is as follows

[0119]

[0120] δx LiDAR Indicates the position and posture state of the laser radar. The Jacobian matrix representing the laser radar posture state, w l represents the observation noise matrix. To project the line feature points to the w system:

[0121]

[0122] Where × represents the cross product. Vector ζ=(l w , m w ) T . Where l is the direction vector of the line, Assumptions and Finally, the uncertainty of the projected point and plane can be used to determine the uncertainty of the point-to-plane distance observation.

[0123]

[0124] Step 2: Point-surface constraint and variance modeling. For plane observation, construct the point-surface observation equation:

[0125]

[0126] is the projection point projected to the anchor coordinate system w; H is the Jacobi matrix for LiDAR pose; Π is the Jacobian ratio matrix of the surface feature points. Similarly, the uncertainty of point surface observation is obtained

[0127]

[0128]

[0129] The implementation basis of each embodiment of the present invention is to implement programmed processing through a device with a processor function. Therefore, in engineering practice, the technical solutions and functions of each embodiment of the present invention are encapsulated into various modules. Based on this reality, on the basis of the above embodiments, an embodiment of the present invention provides a LiDAR point, line and surface feature uncertainty adaptive modeling device, which is used to execute the LiDAR point, line and surface feature uncertainty adaptive modeling method in the above method embodiment.

[0130] See also Figure 8 The device includes: a first main module for acquiring laser radar point cloud data; a second main module for extracting line and surface feature points from non-ground points based on the acquired laser radar point cloud data; a third main module for extracting, merging and tracking line and surface feature landmarks based on the extracted line and surface feature points; a fourth main module for performing point cloud uncertainty modeling based on the point cloud on the obtained line and surface feature landmarks, and performing initialization line and surface feature uncertainty modeling based on the radar points with uncertainty after the point cloud uncertainty modeling, so as to obtain the initialization line and surface features with uncertainty; a fifth main module for screening the key point cloud on the line and surface feature landmarks based on the point cloud on the obtained line and surface feature landmarks according to the line and surface geometric characteristics and the minimum representation method; a sixth main module for constructing point-line and point-surface distance constraints based on the initialization line and surface features with uncertainty and the screened key point cloud, and modeling the observation noise based on variance propagation.

[0131] The LiDAR point, line and surface feature uncertainty adaptive modeling device provided by the embodiment of the present invention adopts the method of Figure 8 Several modules in the proposed method construct a maintainable parameterized local sub-graph by parametrically extracting and tracking line and surface features, and modeling the uncertainty of point clouds and parameterized plane / line features in the local sub-graph, so as to adaptively determine the observation covariance, effectively improving the positioning performance of GNSS / INS / LiDAR in various complex urban environments and improving the LIO variance consistency.

[0132] It should be noted that the device embodiments provided by the present invention are used to implement the methods in the above method embodiments as well as the methods in other method embodiments provided by the present invention. The only difference is that the corresponding functional modules are set, and the principle is basically the same as the principle of the above device embodiments provided by the present invention. As long as the technical personnel in the field refer to the specific technical solutions in other method embodiments on the basis of the above device embodiments, obtain the corresponding technical means and the technical solutions composed of these technical means by combining the technical features, and on the premise of ensuring the practicality of the technical solutions, improve the modules in the above device embodiments to obtain the corresponding device class embodiments, which are used to implement the methods in other method class embodiments. For example:

[0133] Based on the content of the above device embodiment, as a preferred embodiment, in the LiDAR point, line and surface feature uncertainty adaptive modeling device provided in the embodiment of the present invention, the second main module is also used to execute the following instructions:

[0134] Based on the acquired LiDAR point cloud data, the ground points are extracted in combination with the Ground-Detection algorithm;

[0135] Based on the extracted ground points, non-ground points of the LiDAR point cloud data are obtained;

[0136] Based on the obtained non-ground points, curvature threshold segmentation is performed to extract line and surface feature points from the non-ground points.

[0137] Based on the content of the above device embodiment, as a preferred embodiment, in the LiDAR point, line and surface feature uncertainty adaptive modeling device provided in the embodiment of the present invention, the third main module is also used to execute the following instructions:

[0138] Use Line / Plane-Detection algorithm to extract line / plane feature landmarks respectively;

[0139] Based on the extracted line / surface feature landmarks, line and surface features are merged;

[0140] Based on the line-surface feature merging results, data association is performed to find the correspondence between the line / surface features of multiple lidar frames in the sliding window to achieve line-surface feature tracking.

[0141] Based on the content of the above device embodiment, as a preferred embodiment, in the LiDAR point, line and surface feature uncertainty adaptive modeling device provided in the embodiment of the present invention, the fourth main module is also used to execute the following instructions:

[0142] Point cloud uncertainty modeling: The uncertainty of the lidar point in the local lidar frame includes the range uncertainty and the azimuth uncertainty. Let ω iis the measured direction, ω i Azimuth noise on the tangent plane, d i is the depth measurement, is the ranging noise, then

[0143]

[0144] N(ω i )=[N1 N1]∈R 3×2 ω i Orthogonal basis vectors on the orthogonal plane, then the measurement point p i The noise δp i and its covariance for:

[0145]

[0146] in Yes i The orthogonal basis of the tangent plane, represents the cross product, i.e. ω i The antisymmetric matrix of ;

[0147] Initialize line and surface feature uncertainty modeling: by using covariance The estimated pose and The laser radar point Project it into the fixed anchor coordinate system w, and obtain the uncertainty of the projection point through variance propagation,

[0148]

[0149] in, are the i-th point in the LiDAR coordinate system, the w coordinate system, and the ECEF coordinate system respectively; is the rotation at the kth moment uncertainty, It's location uncertainty; further use of the projection point Fit the line and surface features to obtain the line segment parameters LF(l, m) and plane parameters PF(n, q c ) and its uncertainty.

[0150] Based on the content of the above device embodiment, as a preferred embodiment, in the LiDAR point, line and surface feature uncertainty adaptive modeling device provided in the embodiment of the present invention, the fifth main module is also used to execute the following instructions:

[0151] The SVD algorithm is used to calculate the eigenvalue λ of the covariance matrix A of the surface feature point cloud. The plane PF is divided into four sub-planes PF according to the eigenvectors u1 and u2 corresponding to the largest and second largest eigenvalues ​​λ1 and λ2. 1 , pF 2 , pF 3 , pF 4 , average the point cloud on each sub-plane and calculate the center point of each sub-plane in turn and the center of the entire plane q c Together they form a key point cloud on the surface feature landmarks;

[0152] Compute the line segment center point q based on the point cloud on the line feature c , calculate the point on the line With the center point q c The maximum distance D, according to q c , D and the direction vector l of the line segment, calculate the upper and lower endpoints eq1 and eq2 of the line segment to form the key point cloud on the line feature landmark.

[0153] Based on the content of the above device embodiment, as a preferred embodiment, in the LiDAR point, line and surface feature uncertainty adaptive modeling device provided in the embodiment of the present invention, the sixth main module is also used to execute the following instructions:

[0154] Point-line constraints and variance modeling:

[0155] The distance constraint from point to line is as follows

[0156]

[0157] To project the line feature points to the w system:

[0158]

[0159] Where × represents the cross product, vector ζ=(l w , m w ) T , l is the direction vector of the line, Assumptions and The uncertainty of the distance from the point to the plane is determined using the uncertainty of the projected point and the plane.

[0160]

[0161] Point-surface constraints and variance modeling:

[0162] For plane observation, construct the point-surface observation equation,

[0163]

[0164] is the projection point projected to the anchor coordinate system w; is the Jacobian matrix about LiDAR pose; HΠ is the Jacobian matrix about surface feature points. Similarly, the uncertainty of point-surface observation is obtained

[0165]

[0166] The method of the embodiment of the present invention is implemented by relying on electronic devices, so it is necessary to introduce the relevant electronic devices. Based on this purpose, the embodiment of the present invention provides an electronic device, such as Fig. 9 As shown, the electronic device includes: at least one processor, a communication interface, at least one memory, and a communication bus, wherein at least one processor, the communication interface, and at least one memory communicate with each other through the communication bus. At least one processor calls the logic instructions in at least one memory to execute all or part of the steps of the method provided by the aforementioned various method embodiments.

[0167] In addition, when the logic instructions in the at least one memory mentioned above are implemented in the form of a software functional unit and sold or used as an independent product, they are stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or partly in other words, the part that contributes to the prior art or part of the technical solution is embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions for enabling a computer device (a personal computer, a server, or a network device) to perform all or part of the steps of the method described in each method embodiment of the present invention. The aforementioned storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a disk or an optical disk, and various media for storing program codes.

[0168] The device embodiments described above are merely illustrative, wherein the units described as separate components are or are not physically separated, and the components shown as units are or are not physical units, located in one place, or distributed on multiple network units. Some or all of the modules are selected according to actual conditions to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art can understand and implement without creative effort.

[0169] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0170] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0171] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0172] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0173] In summary, the present invention proposes a method and device for adaptive modeling of uncertainty of LiDAR point, line and surface features. The method constructs a maintainable parameterized local subgraph by parametrically extracting and tracking plane / line features, and models the uncertainty of point clouds and parameterized plane / line features in the local subgraph, thereby adaptively determining the observation covariance. The present invention can achieve better integration with other sensors by reasonably modeling the LiDAR variance, effectively improving the GNSS / INS / LiDAR positioning performance in various complex urban environments and improving the inconsistency of the LIO (LiDAR Inertial Odometry, LIO) state covariance.

[0174] The above description is only a specific implementation of the present invention, and the protection scope of the present invention is not limited thereto. Any person familiar with the technology can understand and think of any changes or substitutions within the technical scope disclosed by the present invention, which should be included in the scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A LiDAR point, line and surface feature uncertainty adaptive modeling method, characterized in that: The following steps are involved: Get LiDAR point cloud data; Extract line and surface feature points from non-ground points based on the acquired LiDAR point cloud data; Based on the extracted line and surface feature points, extract, merge and track line and surface feature landmarks; Based on the point cloud on the obtained line and surface feature landmark, point cloud uncertainty modeling is performed, and based on the radar points with uncertainty after point cloud uncertainty modeling, initialization line and surface feature uncertainty modeling is performed to obtain initialization line and surface features with uncertainty; Based on the point cloud on the line and surface feature landmarks obtained, the key point cloud on the line and surface feature landmarks is screened according to the line and surface geometric characteristics and the minimum representation method; Based on the initialized line and surface features with uncertainty and the screened key point cloud, point-line and point-surface distance constraints are constructed, and the observation noise is modeled based on variance propagation.

2. The LiDAR point, line and surface feature uncertainty adaptive modeling method according to claim 1, characterized in that: Based on the acquired LiDAR point cloud data, extract line and surface feature points from non-ground points, including: Based on the acquired LiDAR point cloud data, the ground points are extracted in combination with the Ground-Detection algorithm; Based on the extracted ground points, non-ground points of the LiDAR point cloud data are obtained; Based on the obtained non-ground points, curvature threshold segmentation is performed to extract line and surface feature points from the non-ground points.

3. The LiDAR point, line and surface feature uncertainty adaptive modeling method according to claim 1, characterized in that: Based on the extracted line and surface feature points, line and surface feature landmarks are extracted, merged and tracked, including: Use Line / Plane-Detection algorithm to extract line / plane feature landmarks respectively; Based on the extracted line / surface feature landmarks, line and surface features are merged; Based on the line-surface feature merging results, data association is performed to find the correspondence between the line / surface features of multiple lidar frames in the sliding window to achieve line-surface feature tracking.

4. The LiDAR point, line and surface feature uncertainty adaptive modeling method according to claim 1, characterized in that: Based on the point cloud obtained on the line and surface feature landmark, point cloud uncertainty modeling is performed, and based on the radar points with uncertainty after point cloud uncertainty modeling, initialization line and surface feature uncertainty modeling is performed to obtain initialization line and surface features with uncertainty, including: According to the range uncertainty and azimuth uncertainty of the lidar point in the local lidar frame, the point cloud uncertainty modeling is performed to obtain the lidar point with uncertainty By having covariance The estimated pose and The laser radar point Project it into the fixed anchor coordinate system w, and obtain the uncertainty of the projection point through variance propagation, in, are the i-th point in the LiDAR coordinate system, the w coordinate system, and the ECEF coordinate system respectively; is the rotation at the kth moment uncertainty, It's location uncertainty; further use of the projection point Fit the line and surface features to obtain the line segment parameters LF(l,m) and plane parameters PF(n,q c ) and its uncertainty.

5. The LiDAR point, line and surface feature uncertainty adaptive modeling method according to claim 1, characterized in that: Based on the point cloud on the line and surface feature landmarks, the key point cloud on the line and surface feature landmarks is screened according to the line and surface geometric characteristics and the minimum representation method, including: The SVD algorithm is used to calculate the eigenvalue λ of the covariance matrix A of the surface feature point cloud, and the plane PF is divided into four sub-planes PF according to the eigenvectors u1 and u2 corresponding to the largest and second largest eigenvalues ​​λ1 and λ2. 1 ,PF 2 ,PF 3 ,PF 4 , average the point cloud on each sub-plane and calculate the center point of each sub-plane in turn and the center of the entire plane q c Together they form a key point cloud on the surface feature landmarks; Compute the line segment center point q based on the point cloud on the line feature c , calculate the point on the line With the center point q c The maximum distance D, according to q c , D and the direction vector l of the line segment, calculate the upper and lower endpoints eq1 and eq2 of the line segment to form the key point cloud on the line feature landmark.

6. The LiDAR point, line and surface feature uncertainty adaptive modeling method according to claim 1, characterized in that: Construct point-line and point-surface distance constraints, and model observation noise based on variance propagation, including: Point-line constraints and variance modeling: The distance constraint from point to line is as follows To project the line feature points to the w system: Where × represents the cross product, vector ζ=(l w ,m w ) T , l is the direction vector of the line, Assumptions and The uncertainty of the projected point and plane is used to determine the uncertainty of the distance observation from the point to the plane; Point-surface constraints and variance modeling: For plane observation, construct the point-surface observation equation, is the projection point projected to the anchor coordinate system w; H is the Jacobi matrix for LiDAR pose; ∏ is the Jacobian ratio matrix of the surface feature points. Similarly, the uncertainty of point surface observation is obtained 7. A LiDAR point, line and surface feature uncertainty adaptive modeling device, characterized in that: include: The first main module is used to obtain laser radar point cloud data; The second main module is used to extract line and surface feature points from non-ground points based on the acquired laser radar point cloud data; The third main module is used to extract, merge and track line and surface feature landmarks based on the extracted line and surface feature points; The fourth main module is used to perform point cloud uncertainty modeling based on the point cloud on the line and surface feature landmarks obtained, and to perform initialization line and surface feature uncertainty modeling based on the radar points with uncertainty after the point cloud uncertainty modeling, so as to obtain the initialization line and surface features with uncertainty; The fifth main module is used to screen and obtain key point clouds on the line-surface feature landmarks based on the point clouds on the line-surface feature landmarks obtained according to the line-surface geometric characteristics and the minimum representation method; The sixth main module is used to construct point-line and point-surface distance constraints based on the initialized line and surface features with uncertainty and the screened key point cloud, and to model the observation noise based on variance propagation.

8. An electronic device, characterized in that: include: At least one processor, at least one memory and a communication interface; wherein the processor, memory and communication interface communicate with each other; The memory stores program instructions executed by the processor, and the processor calls the program instructions to execute the method according to any one of claims 1 to 6.

9. A non-transitory computer-readable storage medium, characterized in that: The non-transitory computer-readable storage medium stores computer instructions, which cause the computer to execute the method of any one of claims 1 to 6.

Images