A LiDAR point, line, and surface feature uncertainty adaptive modeling method and device

Abstract

This paper proposes a method and device for adaptively modeling the uncertainty of LiDAR point, line, and surface features. This method constructs a maintainable parameterized local subgraph by parametrically extracting and tracking plane / line features. The uncertainty of the point cloud and parameterized plane / line features in the local subgraph is then modeled, thereby adaptively determining the observation covariance. By rationally modeling LiDAR variance, this method enables better integration with other sensors, effectively improving GNSS / INS / LiDAR positioning performance in various complex urban environments and addressing the inconsistency of LiDAR Inertial Odometry (LIO) state covariance.

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 advancement of artificial intelligence, robotics, and the Internet of Things (IoT), society has entered the intelligent era. Various autonomous mobile robots, including drones (airborne), autonomous vehicles (cars), and unmanned vessels (ships), are rapidly developing, fulfilling diverse and complex human tasks and freeing up human labor. For robots to complete tasks autonomously, their core technologies include navigation and positioning, environmental perception, planning and decision-making, and control, with navigation and positioning being the foundational module. Currently, most robots are equipped with various sensors to emulate humans. These sensors can be divided into two categories: positioning and attitude sensors, such as GNSS (Global Navigation Satellite System), IMU (Inertial Measurement Unit), and odometry; and environmental perception sensors, such as cameras, LiDAR (Light Detection and Ranging), and millimeter-wave radar. These sensors are evolving towards fusion, and multi-source information fusion has become a research hotspot in robotics. LiDAR (Light Detection and Ranging) can detect rich geometric information in the environment and is increasingly used for navigation and positioning. Due to the complementary nature of GNSS, INS, and LiDAR, the fusion of these three sensors for positioning has been widely studied. In multi-source fusion positioning, modeling the uncertainty (covariance) of each sensor is crucial. While GNSS and IMU covariance modeling is mature, LiDAR covariance modeling is less so. Specifically, the covariance of the raw point cloud, the covariance of the local map, and the covariance of the observations are not accurately modeled, leading to suboptimal 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, which cannot form common view constraints for LiDAR multi-frame data; ② There is a lack of research on reasonably determining the variance of LiDAR point, line and surface features, which makes it difficult to perform variance modeling on LiDAR observations, easily leading to inconsistent LiDAR variance. Summary of the Invention

[0004] To address the above problems, the present invention provides a method and device for adaptive modeling of LiDAR point, line, and surface feature uncertainties. By parametrically extracting and tracking line and surface features, a maintainable parametric local subgraph is constructed. The uncertainties of the point cloud and parametric plane / line features in the local subgraph are modeled, thereby adaptively determining the observation covariance. This effectively improves the positioning performance of GNSS / INS / LiDAR in various complex urban environments and improves the LIO variance consistency.

[0005] According to one aspect of the present invention, a method for adaptive modeling of uncertainty of LiDAR point, line and surface features 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 obtained 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;

[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 are extracted from non-ground points, including:

[0013] Extract ground points based on the acquired LiDAR point cloud data and combined 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] Using Lin e / Plan e -Detection algorithm extracts line / surface feature landmarks respectively;

[0018] Merge line and surface features based on the extracted line / surface feature landmarks;

[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 within 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 point cloud uncertainty modeling, initialization line and surface feature uncertainty modeling is performed to obtain initialization line and surface features with uncertainty, including:

[0021] According to the ranging 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 Estimated pose and The lidar 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 obtained point cloud on the line and surface feature landmarks, key point clouds on the line and surface feature landmarks are screened according to the line and surface geometric characteristics and the minimum representation method, 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 q of the entire plane c Together they form a key point cloud on the surface feature landmark;

[0027] The line segment center point q is calculated based on the point cloud on the line feature c , calculate the points 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 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] Line feature points projected onto 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 onto 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, a device for adaptively modeling uncertainty of LiDAR point, line, and surface features is provided, comprising:

[0042] The first main module is used to obtain lidar 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 lidar 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 obtained line and surface feature landmark, and perform initialization line and surface feature uncertainty modeling based on the radar points with uncertainty after the point cloud uncertainty modeling, to obtain the initialization line and surface features with uncertainty;

[0046] The fifth main module is used to screen the key point clouds on the line-surface feature landmarks based on the obtained point clouds on the line-surface feature landmarks 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 invention, 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 to be executed by the processor, and the processor calls the program instructions to execute the described method.

[0049] According to one aspect of the present disclosure, a non-transitory computer-readable storage medium is provided, wherein the non-transitory computer-readable storage medium stores computer instructions, wherein the computer instructions cause 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 LIO variance consistency. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

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

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

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

[0057] Figure 6 This is a flow chart for modeling uncertainty of line and surface features provided by an embodiment of the present invention.

[0058] Figure 7 This is a flowchart for screening key point clouds of line and surface features provided by an embodiment of the present invention.

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

[0060] Figure 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 the point cloud and parametric plane / line features in the local subgraph. Then, the key point cloud on the line and surface feature landmarks is screened, and finally, the key point cloud is used to construct point-line / surface distance constraints, thereby improving computational efficiency while adaptively determining the observation covariance. By rationally modeling the LiDAR variance, it is possible to achieve better integration with other sensors, effectively improving the GNSS / INS / LiDAR positioning performance in various complex urban environments and improving 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] Common mechanical LiDARs emit each laser point at a different time, so the laser point cloud within a frame is not in the same local coordinate system. Furthermore, due to environmental factors such as atmospheric pressure, temperature, humidity, and the reflective properties of the target object, the collected laser point cloud has certain errors. By performing preprocessing operations such as filtering, denoising, and segmentation on the input point cloud data obtained by the LiDAR, new, more accurate point cloud data is obtained.

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

[0066] Since 3D LiDAR sensors 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, there is no need to process each point separately. Using the original point cloud to extract line, surface and other feature information 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 point extraction is performed using the Ground-Detection algorithm on the pre-processed point cloud data, such as Figure 2 As shown in the figure, 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 t time, 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, and are represented 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 assigned to the edge feature point set; if the c value is less than the threshold, it is assigned 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 direction of the line and the normal vector of the surface 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. The 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 closest to p1 on the (i+2)-th scan line. If the distance between p2 and the scan line is less than a threshold (0.1mm), 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 shown below, 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, the KNN algorithm is used to search for its k nearest neighboring points outside the i-th scan line. Then, the SVD algorithm is used 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 whose distance is 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 reusing observation information and improve computational 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 linear residuals and plane residuals 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 will further focus on the second term of the residual T represents the normal vector n of the i-th plane 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 k-1 and k, the plane indexed as i in the kth frame can be projected onto the k-1th frame, and its nearest plane feature indexed as 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 j-th plane in the k-1-th 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. 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. It can be defined as follows:

[0086]

[0087] The physical meaning of the first term is to convert the line direction vector of the kth frame to the k-1th frame and calculate the difference with the direction vector of the k-1th frame. The second term 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 line.

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

[0089] Lin e / Plan e -Detection extracts and merges the point cloud on the tracked line and surface feature landmarks to perform uncertainty modeling, projects the point cloud on the common view landmarks into 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, ranging 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 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. Estimated pose and The lidar point Project it into the fixed anchor coordinate system w and obtain the uncertainty of the projected 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 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, and λ3, from largest to smallest, and their corresponding eigenvectors are u1, u2, and u3. The partial derivatives of l and m with respect to each point can be calculated and used to calculate the Jacobian matrix.

[0102]

[0103] For the plane we define the plane parameters 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 a point p on the plane i With the plane parameters PF(n,q c ) function, λ3, λ 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, theoretically 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 line to construct observation constraints. Using all point cloud observations will not only reduce computational efficiency but also lead to overly optimistic state covariance estimation. Therefore, the present invention filters the point clouds on the line and surface feature landmarks extracted and merged after Line / Plane-Detection tracking, and filters the key point clouds on the line and surface feature landmarks based on the line and surface geometric characteristics and the minimum representation method to achieve point cloud downsampling, such as Figure 7 shown.

[0110] Step 1: Downsampling of surface feature point cloud. First, use the SVD algorithm to calculate the eigenvalue λ of the surface feature point cloud covariance matrix A, and then divide the plane PF 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 q of the entire plane 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] Based on 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 state of the lidar, The Jacobian matrix representing the laser radar pose state, w l represents the observation noise matrix. Line feature points projected onto 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 onto the anchor coordinate system w; H is the Jacobian matrix of LiDAR pose; Π is the Jacobian ratio matrix of the surface feature points. Similarly, the uncertainty of the point surface observation is obtained

[0127]

[0128]

[0129] The implementation of each embodiment of the present invention is based on programmed processing performed by a device with processor functionality. Therefore, in practical engineering, the technical solutions and functions of each embodiment of the present invention are encapsulated into various modules. Based on this reality, and in addition to the aforementioned embodiments, an embodiment of the present invention provides a device for adaptive modeling of LiDAR point, line, and surface features for uncertainty. This device is used to implement the LiDAR point, line, and surface feature uncertainty adaptive modeling method described in the aforementioned method embodiment.

[0130] See also Figure 8 The device includes: a first main module for acquiring lidar point cloud data; a second main module for extracting line and surface feature points from non-ground points based on the acquired lidar 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 to obtain initialization line and surface features with uncertainty; a fifth main module for screening key point clouds 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 clouds, 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 aims to solve the main problems existing in the current GNSS / INS / LiDAR fusion positioning. Figure 8 Several modules in the system construct maintainable parameterized local subgraphs by parametrically extracting and tracking line and surface features, and model the uncertainty of point clouds and parameterized plane / line features in the local subgraphs, 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.

[0132] It should be noted that the device embodiments provided by the present invention are not only used to implement the methods in the above-mentioned method embodiments, but also used to implement the methods in other method embodiments provided by the present invention. The only difference is the setting of corresponding functional modules. The principles thereof are basically the same as those of the above-mentioned device embodiments provided by the present invention. As long as those skilled in the art refer to the specific technical solutions in other method embodiments on the basis of the above-mentioned device embodiments, obtain corresponding technical means and technical solutions composed of these technical means by combining technical features, and on the premise of ensuring the practicality of the technical solutions, improve the modules in the above-mentioned device embodiments to obtain corresponding device-type embodiments, which are used to implement the methods in other method-type embodiments. For example:

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

[0134] Extract ground points based on the acquired LiDAR point cloud data and combined 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, the LiDAR point, line, and surface feature uncertainty adaptive modeling device provided in the embodiment of the present invention is further configured to execute the following instructions:

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

[0139] Merge line and surface features based on the extracted line / surface feature landmarks;

[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 within the sliding window to achieve line-surface feature tracking.

[0141] Based on the content of the above device embodiment, as a preferred embodiment, the LiDAR point, line, and surface feature uncertainty adaptive modeling device provided in the embodiment of the present invention is further configured 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 Estimated pose and The lidar 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, the LiDAR point, line, and surface feature uncertainty adaptive modeling device provided in the embodiment of the present invention is further configured 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 q of the entire plane c Together they form a key point cloud on the surface feature landmark;

[0152] The line segment center point q is calculated based on the point cloud on the line feature c , calculate the points 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, the LiDAR point, line, and surface feature uncertainty adaptive modeling device provided in the embodiment of the present invention is further configured 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] Line feature points projected onto 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 onto 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 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 Figure 9 As shown, the electronic device includes: at least one processor, a communications interface, at least one memory, and a communications bus, wherein the at least one processor, the communications interface, and the at least one memory communicate with each other via the communications bus. The at least one processor invokes logic instructions in the at least one memory to execute all or part of the steps of the methods provided in the aforementioned method embodiments.

[0167] In addition, when the logic instructions in the at least one memory 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 the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (a personal computer, a server, or a network device) to execute 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), a random access memory (RAM), a magnetic disk or an optical disk, various media for storing program codes.

[0168] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, located in one place or distributed across multiple network units. Some or all of these modules may be selected based on practical needs to achieve the objectives of this embodiment. Persons of ordinary skill in the art will understand and implement these embodiments without inventive effort.

[0169] It will be understood by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, 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 magnetic 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 flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, 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 flowcharts and / or block diagrams. 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 that can direct a computer or other programmable data processing device to work 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 The 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 operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0173] In summary, the present invention proposes a method and device for adaptive modeling of uncertainty in LiDAR point, line, and surface features. This method constructs a maintainable parameterized local subgraph by parametrically extracting and tracking plane / line features, and models the uncertainty of the point cloud and parameterized plane / line features in the local subgraph, thereby adaptively determining the observation covariance. By rationally modeling the LiDAR variance, the present invention can 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 (LiDAR Inertial Odometry, LIO) state covariance.

[0174] The above description is only a specific embodiment 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 replacements 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 by: 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 obtained 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; 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, line and surface feature points are extracted from non-ground points, including: Extract ground points based on the acquired LiDAR point cloud data and combined 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; Merge line and surface features based on the extracted line / surface feature landmarks; 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 within 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. Based on the radar points with uncertainty after point cloud uncertainty modeling, initial line and surface feature uncertainty modeling is performed to obtain initial line and surface features with uncertainty, including: According to the ranging 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 Estimated pose and The lidar 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 obtained 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, u2 corresponding to the largest and second largest eigenvalues ​​λ1, λ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 q of the entire plane c Together they form a key point cloud on the surface feature landmark; The line segment center point q is calculated based on the point cloud on the line feature c , calculate the points 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 Line feature points projected onto 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 onto the anchor coordinate system w; H is the Jacobian matrix of LiDAR pose; ∏ is the Jacobian ratio matrix of the surface feature points. Similarly, the uncertainty of the 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 lidar point cloud data; The second main module is used to extract line and surface feature points from non-ground points based on the acquired lidar 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 obtained line and surface feature landmark, and perform initialization line and surface feature uncertainty modeling based on the radar points with uncertainty after the point cloud uncertainty modeling, to obtain the initialization line and surface features with uncertainty; The fifth main module is used to screen the key point clouds on the line-surface feature landmarks based on the obtained point clouds on the line-surface feature landmarks 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