A mapping and positioning method based on point cloud manifold analysis, electronic device and storage medium

Abstract

The present invention discloses a mapping and positioning method, electronic device and storage medium based on point cloud manifold analysis, which obtains point cloud data of the environment through a laser radar, converts the point cloud data into a coordinate system, and obtains point cloud data in a vehicle coordinate system; constructs a factor optimization objective function using the point cloud data in the vehicle coordinate system, solves the factor optimization objective function, deletes abnormal point clouds caused by interference from dynamic objects and high-brightness mirror reflection objects, and obtains updated and optimized point cloud data; constructs a ground segmentation binary classification model using the point cloud data after deleting the abnormal points, solves the binary classification model, and obtains a ground point set and a non-ground point set after ground segmentation; uses the ground point set and the non-ground point set after segmentation to estimate the current position of the vehicle and finally generate an environmental map. The method of the present invention optimizes map construction by analyzing the point cloud manifold features, improves the accuracy and robustness of the mapping system, and better adapts to complex scenes.

Description

Technical Field

[0001] The invention relates to a mapping and positioning method, electronic equipment and storage medium based on point cloud manifold analysis, and belongs to the technical field of positioning and navigation. Background Art

[0002] In the existing laser SLAM (Simultaneous Localization and Mapping) mapping and positioning technology, abnormal point cloud removal and ground point segmentation are two key links, which directly determine the accuracy of SLAM mapping. However, the current technology has the following problems that need to be solved:

[0003] Interference from dynamic objects and high-brightness specular reflection objects: In real environments, the presence of dynamic objects and high-brightness specular reflection objects will have a significant impact on the laser point cloud mapping process. Existing technologies only regard laser point cloud data as a collection of scattered points, but fail to regard it as a whole - a point cloud manifold. Therefore, there is a lack of utilization of the overall geometric continuity and local motion characteristics of the point cloud, making it difficult to effectively remove these dynamic point clouds and high-brightness reflection noise points, resulting in a suboptimal mapping effect.

[0004] Inaccurate segmentation of ground points and non-ground points: Most existing map construction processes only focus on static single-point geometric features such as point cloud distribution location and curvature, while ignoring the connectivity and elevation differences between ground points and non-ground points. This processing method often leads to blurred segmentation details at the junction of ground points and non-ground points, and makes the map prone to drift in the Z-axis direction, thus affecting the accuracy and reliability of mapping.

[0005] In summary, the existing mapping and positioning technology based on laser SLAM has obvious deficiencies in the interference suppression of dynamic objects and high-brightness mirror reflection objects, the accuracy of ground point segmentation, and the control of Z-axis drift. These problems seriously affect the mapping accuracy of laser SLAM. Summary of the invention

[0006] Purpose: In order to overcome the deficiencies in the prior art, the present invention provides a mapping and positioning method based on point cloud manifold analysis, an electronic device and a storage medium.

[0007] Technical solution: To solve the above technical problems, the technical solution adopted by the present invention is:

[0008] In the first aspect, a mapping and positioning method based on point cloud manifold analysis specifically includes:

[0009] Obtain point cloud data of the environment, transform the point cloud data into a coordinate system, and obtain point cloud data in a vehicle coordinate system.

[0010] The factor optimization objective function is constructed using the point cloud data in the vehicle coordinate system, the factor optimization objective function is solved, and the abnormal point clouds caused by the interference of dynamic objects and high-brightness mirror reflection objects are deleted to obtain the updated and optimized point cloud data.

[0011] A ground segmentation binary classification model is constructed using the point cloud data after deleting abnormal points. The binary classification model is solved to obtain the optimized ground point set and non-ground point set.

[0012] The optimized ground point set and non-ground point set are used to estimate the vehicle's current position and generate a map.

[0013] As a preferred solution, the factor optimization objective function expression is as follows:

[0014]

[0015] in, represents the factor optimization objective function, is proportional to, represents the product, represents the dynamic factor, represents the reflection intensity factor, Indicates the location of the vehicle at each moment in the constructed image, represents the updated and optimized point cloud data to be requested, Represents the point cloud data after registration, Represents the observed position data of a dynamic point cloud. represents the observed laser reflection intensity, Represents the preset material reflection characteristic matrix, Indicates the ambient light brightness.

[0016] As a preferred solution, the method for solving the factor optimization objective function to obtain the point cloud data after deleting the abnormal points specifically includes:

[0017] Factoring the optimization objective function Maximization is converted into energy function minimize.

[0018] Use nonlinear optimization algorithm to minimize the energy function , during the minimization process:

[0019] If a point has a dynamic factor residual in consecutive frames If the dynamic factor is greater than the threshold, the point is considered as a dynamic point and removed from the point cloud.

[0020] If the residual reflection intensity at a point If the value is greater than the reflection intensity factor threshold, the point is considered as a high-brightness reflection noise and removed from the point cloud.

[0021] During the iterative optimization process, dynamic points and high-brightness reflection noise points are continuously removed until the energy function Convergence or reaching a predetermined number of iterations.

[0022] The last set of vehicle positions to be iteratively optimized and static point cloud positions Considered as the optimal point cloud dataset.

[0023] As a preferred solution, the energy function The expression is as follows:

[0024]

[0025] in, Represents the weight, which is used to dynamically control the influence of the two factors in iterative optimization. is the robust kernel function, represents the dynamic factor residual, Represents the reflected intensity residual.

[0026] As a preferred solution, the ground segmentation binary classification model expression is as follows:

[0027]

[0028] in, represents the likelihood function of the point cloud ground point, represents the conditional probability density function, represents the product, represents the random variable of the set of ground points in the nth smallest processing unit, Represents the relevant parameters of the ground point set in the nth minimum processing unit.

[0029] As a preferred solution, the method of solving the binary classification model to obtain the ground point set and the non-ground point set specifically includes:

[0030] Get the initial value of the seed ground point selected by each minimum processing unit .

[0031] Set the initial value of the current ground plane to , which is continuously updated during the iteration process. After m iterations, the final ground point set is selected. .

[0032] The final ground point collection Substitute into the binary classification model and solve , according to the final discriminant of the two categories , and obtain the optimized ground point set and non-ground point set.

[0033] in, represents the set of ground points, n represents the minimum processing unit, N represents the set of minimum processing units, represents the union symbol, [·] represents the Iverson bracket, Represents the set of ground points in the nth smallest processing unit.

[0034] As a preferred embodiment, the The expression is as follows:

[0035]

[0036] in, represents the connectivity element of the i-th point, Represents the elevation gradient feature of the i-th point.

[0037] As a preferred solution, the connectivity element of the i-th point The expression is as follows:

[0038]

[0039] in, Yes The connectivity metric value of is an empirical threshold.

[0040]

[0041] in, is the point in the point cloud The horizontal distance is less than the threshold The number of neighboring points. Represents the norm of 2. represents the natural exponential function, Indicate point The normal vector of Indicates neighboring points The normal vector of is the smoothing parameter.

[0042] As a preferred solution, the elevation gradient element of the i-th point The expression is as follows:

[0043] ψ ϕ( p i ) = 1+ e [G( p i ) - G th ] - 1

[0044] in, represents a natural constant, Indicate point The local variance of the height gradient, is the threshold of the local variance of the height gradient.

[0045] As a preferred solution, the point The local variance of the height gradient The expression is as follows:

[0046]

[0047] in, To analyze the elevation gradient in the x direction of the kth grid in the window, To analyze the elevation gradient in the y direction of the kth grid in the window, is the number of grids in the analysis window, is the average elevation gradient in the x direction of the analysis window, It is the average elevation gradient in the y direction of the analysis window.

[0048] In a second aspect, a computer-readable storage medium stores a computer program, which, when executed by a processor, implements a mapping and positioning method based on point cloud manifold analysis as described in any one of the first aspects.

[0049] According to a third aspect, a computer device includes:

[0050] Memory, used to store instructions.

[0051] A processor is used to execute the instructions so that the computer device performs the operations of a mapping and positioning method based on point cloud manifold analysis as described in any one of the first aspects.

[0052] Beneficial effect: The present invention provides a mapping and positioning method, electronic device and storage medium based on point cloud manifold analysis. In the laser SLAM mapping process, the factor graph is used to fuse the dynamic information and reflection intensity information between point cloud frames, remove the point cloud of dynamic objects and high-brightness reflection noise points, and then use the point cloud connectivity and elevation gradient analysis to achieve refined segmentation of ground points, and finally complete high-precision mapping.

[0053] The method of the present invention optimizes map construction, improves the accuracy and robustness of the mapping system, and better adapts to complex scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 The present invention is a flowchart of an embodiment of a mapping and positioning method based on point cloud manifold analysis.

[0055] Figure 2 This is a schematic diagram of the structure of the elevation gradient point cloud analysis window (3×3 window).

[0056] Figure 3This is a ground segmentation effect diagram of the method of the present invention.

[0057] Figure 4 It is a schematic diagram comparing the overall and local mapping effects of the prior art and the method of the present invention, wherein: Figure 4 (a) shows the overall and local mapping effects of the LIO-SAM method. Figure 4 (b) shows the overall and local mapping effects of the LeGO-LOAM method. Figure 4 (c) is a diagram showing the overall and local mapping effects of the method of the present invention.

[0058] Figure 5 The figure is a schematic diagram comparing the absolute posture error bar graphs of the prior art and the method of the present invention. DETAILED DESCRIPTION

[0059] The following is a clear and complete description of the technical solutions in the examples of the present invention in conjunction with the accompanying drawings in the examples of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the protection scope of the present invention.

[0060] The present invention will be further described below in conjunction with specific embodiments.

[0061] Embodiment 1:

[0062] This embodiment introduces a mapping and positioning method based on point cloud manifold analysis. Figure 1 As shown, specifically including:

[0063] Step 1: Obtain the point cloud data of the environment, convert the point cloud data into a coordinate system, and obtain the point cloud data in the vehicle coordinate system.

[0064] Furthermore, the step 1 specifically includes:

[0065] Step 1.1: The laser radar collects point cloud data of the environment at a fixed time interval (e.g. 0.1 second). Each point in the point cloud data is represented as , ,in, is the x-axis coordinate value and y-axis coordinate value of the point on the horizontal plane, It is the elevation value of the point (the elevation value refers to the distance from a point to the leveling base along the plumb line).

[0066] Step 1.2: Use the rotation matrix to transform the point cloud data and translation vectors Perform coordinate system conversion to obtain point cloud data in the vehicle coordinate system.

[0067] Among them, the point cloud data in the vehicle coordinate system The expression is as follows:

[0068]

[0069] in, is the rotation matrix, is the translation vector.

[0070] Since there may be position and height deviations in the actual installation process of the LiDAR, in order to improve the accuracy of mapping, the collected point cloud data needs to be converted into a unified vehicle coordinate system (the vehicle coordinate system takes the geometric center of the vehicle as its origin).

[0071] Step 2: Construct a joint probability model for the point cloud data in the vehicle coordinate system obtained in step 1 Remove outliers from the point cloud to obtain processed point cloud data. Fuse the inter-frame motion information of the point cloud (the motion of image features in consecutive frames) with the reflection intensity data of the LiDAR (combined with the material reflection characteristic matrix to describe the reflection characteristics of different object surfaces). By maximizing the joint posterior probability and using the robust kernel function to suppress outliers, the accurate removal of abnormal point cloud return values ​​caused by dynamic objects and high-brightness reflections is achieved to ensure the quality of mapping.

[0072] Step 2.1: Inter-frame point cloud registration

[0073] exist and At this moment, the point cloud data obtained from step 1 Get a frame of point cloud set in each frame, denoted as and . For the moment and The point cloud of is registered using the iterative closest point (ICP) algorithm, and the registered point cloud data is recorded as Z, so that the distance between the corresponding point pairs of the two frames of point cloud is minimized to eliminate the environmental point cloud offset caused by the vehicle's own motion in the kinematic analysis. The specific steps are as follows:

[0074] for For each point in The nearest neighbor point in .

[0075] based on and The corresponding point pairs in the two frames are used to calculate the rotation matrix and translation vector that can minimize the distance between the corresponding point pairs in the point clouds of the two frames.

[0076] Use the obtained rotation matrix and translation vector to update location.

[0077] Repeat the above steps until convergence or the predetermined number of iterations is reached, and keep updating The final position is the point cloud data from which the vehicle's own motion deviation has been eliminated, recorded as .

[0078] Step 2.2: Define dynamic factors and dynamic factor residuals:

[0079] Dynamic Factor Indicates that at a given vehicle position and static feature point positions In the case of a certain dynamic point cloud position data observed This probability models the matching of image features between consecutive frames. Specifically, by tracking the projection to the feature point in the image The pixel position changes on the image are used to capture dynamic information.

[0080] In the same relative coordinate system, all static objects are in a dynamic amount, while moving objects will have a prominent dynamic amount. Thus, dynamic point clouds are removed. Quantization is performed using the dynamic factor residual:

[0081] Dynamic factor residuals .in, , Respectively represent the point cloud feature points at time and The pixel coordinates of . Represents the projection function, which is used to map a 3D point cloud to the image plane. is the standard deviation of the measurement error of the dynamic factor. and are the rotation parameters and translation parameters obtained from the point cloud registration in step 2.1. If the value exceeds a certain threshold (using a robust kernel function to handle outliers), it indicates that the observation may come from a dynamic object.

[0082] Step 2.3: Define the reflection intensity factor and the reflection intensity factor residual:

[0083] Reflection intensity factor Represents the preset material reflection characteristic matrix and ambient light brightness In the case of probability. It is The laser reflection intensity at each point is .in, is the material reflection characteristic matrix, which is determined by the object material. Indicates that the object is at the laser radar wavelength , angle of incidence The reflectivity at .

[0084] Under the same ambient lighting, most objects will have similar point cloud return values. Objects with specular reflection will have high brightness return values, which will interfere with the modeling of surrounding point clouds and cause blurred details. Therefore, the difference in reflection intensity under ambient lighting is used to identify and remove high brightness noise points caused by specular reflection. Therefore, the reflection intensity factor residual is used for quantification:

[0085] Define the reflection intensity factor residual . is the standard deviation of the measurement error of the reflection intensity. If the value is too large, it means that the reflection intensity of the point does not match the normal material model under the surrounding environment lighting, which may be due to abnormal returns caused by mirror reflection objects.

[0086] Step 2.4: Factor optimization model construction

[0087] Constructing factor optimization objective function The parameters to be solved and hidden variables With the acquired observation data They are linked in the form of probability factors and transformed into a maximum a posteriori probability (MAP) problem.

[0088] The composition is as follows:

[0089]

[0090] Among them, the parameters Indicates the location of the vehicle at each moment in the constructed image. Hidden variable Represents the updated and optimized point cloud data to be requested (mainly focusing on the location of the static feature point cloud in the environment). Represents the point cloud set obtained after processing in step 2.1. stands for "proportional to", meaning that the expression on the right is a different factor of the probability distribution on the left. It represents the product, and multiplication operation is performed on all related variables. The overall objective function consists of two factors: dynamic factor and reflection intensity factor. represents the dynamic factor described in step 2.2, represents the reflection intensity factor as described in step 2.3.

[0091] When the overall objective function When the maximum value is reached, the influence of the two factors is minimized, and the optimal vehicle position is obtained. And the environmental point cloud features The factor optimization process is the objective function maximization process.

[0092] Step 2.5: Use the method of minimizing the energy function to solve the maximum a posteriori probability problem and set the objective function Maximization is converted into energy function The energy function is directly derived from the joint posterior probability, and the consistency of the optimization objective is ensured by converting the probability in negative logarithmic form into energy representation.

[0093] Define the energy function: Unify the dynamic residual and the reflection intensity residual into a unified model, and introduce a robust kernel function to obtain the overall energy function:

[0094]

[0095] in, Used to adjust the weight of the dynamic factor and the reflection intensity factor. It is a robust kernel function used to process residuals and reduce the impact of outliers (the Huber kernel is used here). The Huber kernel function is defined as follows to process residuals and reduce the impact of outliers:

[0096] is a threshold that controls the transition point from square loss to linear loss.

[0097] Step 2.6: Use a nonlinear optimization algorithm (such as the Loewenberg-Marquardt incremental algorithm) to minimize the energy function . During minimization:

[0098] Dynamic point removal: If a point has a large displacement in consecutive frames (i.e. is greater than the dynamic factor threshold), it is considered as a dynamic point and removed from the point cloud.

[0099] High brightness reflection noise removal: If the reflection intensity residual of a point If it is greater than the reflection intensity factor threshold, it means that the reflection intensity of the point does not match the normal material model of other objects reflected under ambient light. It may be an abnormal return caused by specular reflection, so it is removed from the point cloud.

[0100] During the iterative optimization process, dynamic points and high-brightness reflection noise points that are continuously assigned residual values ​​greater than the threshold are continuously eliminated until the energy function Convergence or reaching a predetermined number of iterations. In each iteration, the vehicle position is updated and static point cloud positions , and remove dynamic points and high-brightness reflective points based on the residual.

[0101] The last set of vehicle positions to be iteratively optimized and static point cloud positions Considered as the optimal point cloud dataset, denoted as . The purpose of building a high-quality point cloud dataset is achieved by using only static point clouds that are not affected by high-brightness noise.

[0102] Step 3: The point cloud data obtained after the outliers in step 2 are removed Perform ground segmentation to obtain ground point cloud set , non-ground point cloud set .in, That is all the point cloud data .

[0103] Furthermore, the step 3 specifically includes:

[0104] Step 3.1: Take the point cloud acquired at time t Divide into N minimum processing units from near to far, where the initial value of the seed ground point selected by each minimum processing unit is .

[0105] Among them, the initial value of the seed ground point The expression is as follows:

[0106]

[0107] in, Indicates the elevation value of the point with the lowest elevation in the current minimum processing unit; It is the elevation offset threshold set based on experience. represents the kth point, is the nth minimum processing unit, represents the elevation value of the kth point, represents conditional probability.

[0108] Step 3.2: As the current ground plane, continue to iteratively check Whether the remaining points in belong to ground points.

[0109] That is, the initial value of the current ground plane is , which is continuously updated during the iteration process.

[0110] After m iterations, the final ground point set is selected and recorded as : .

[0111] in, Represents the normal vector parameters of the current ground plane after m iterations, It represents the coincidence parameter between the kth point to be investigated and the normal vector of the current ground plane after m iterations. is the distance threshold.

[0112] In the m-times selection process, All the remaining point cloud data in the calculation and When the difference is less than the set threshold , that is, the normal vector coincidence degree between the inspection point and the current plane is high, it can be considered that the point belongs to the current plane, so it is added middle.

[0113] Step 3.3: For further removal For the non-ground points that may appear in the image, a binary classification module for ground likelihood estimation is designed. is the likelihood function of the point cloud ground point, then That is, given the parameters Under the condition of Equal to the conditional probability density function .in, represents conditional probability. and Respectively represent the nth minimum processing unit The ground point set in The random variables and associated parameters.

[0114] Step 3.4: There are two major factors to determine whether a point is a ground point: connectivity factor and elevation gradient factor. They are respectively . Then the ground point set The probability density function of whether each point belongs to a ground point can be recorded as . represents the connectivity element of the i-th point, Represents the elevation gradient feature of the i-th point.

[0115] Step 3.5: Because the initial seed points are selected as ground points, the ground points meet the connectivity principle: the normal vectors of adjacent points should remain relatively consistent. That is, the judgment of connectivity depends on the Euclidean distance and the consistency of the normal direction between points. Therefore, the connectivity factor function is defined as : .

[0116] in, Yes The connectivity metric value of is calculated as follows:

[0117] .

[0118] in, is the point in the point cloud The horizontal distance is less than the threshold The number of neighboring points. For measuring points and Point The degree of change of the normal vector between . is a smoothing parameter that controls the weight decay speed of neighboring points. It is set according to the average distance between points in the point cloud. Specifically, first calculate the mean of the Euclidean distances of all point pairs. , then Set to A certain proportion, for example: .in, It is an empirical coefficient, usually between 0.5 and 2, and is adjusted according to the actual situation during the mapping process. , Represent the midpoint of the point cloud and its neighboring points The normal vector of Represents the norm of 2. represents the natural exponential function.

[0119] It is an empirical threshold, and the appropriate connectivity threshold is selected according to the actual environment size.

[0120] Therefore, for The points in From the expression, we can see that when the connectivity measure value is greater than a certain threshold, it is regarded as a ground point.

[0121] Step 3.6: Design elevation gradient variance factor function : In actual applications, there may be a small number of initial seed point selection errors, that is, the points of the above connectivity elements are not necessarily all ground points. For example, when there is an error in the seed point selection height, points on the desktop and the roof of the car will be mistaken for ground points. Therefore, it is necessary to use the elevation gradient to realize ground point correction and identify the ground points that meet the connectivity element function. In the case of , points with large height changes within a certain local range are removed from the ground points. Therefore, the elevation gradient variance factor function is designed : ψ ϕ( p i ) = 1+ e [G( p i ) - G th ] - 1 .

[0122] The value of the function is between 0 and 1. near When , the function value is close to 0.5, indicating uncertainty; when Significantly less than When , the function value is close to 1, indicating that the point is a ground point; when Significantly greater than When , the function value is close to 0, indicating that the point is a non-ground point. Yes The local variance of the height gradient. is the threshold of the local variance of the height gradient. Represents a natural constant.

[0123] Step 3.7: Acquisition and calculation of elevation gradients u and v: The elevation gradients u and v represent the rate of change of the elevation z of the point cloud in the x and y directions in the plane coordinate system within the analysis window.

[0124] Select a 3×3 window as the elevation gradient point cloud analysis window, such as Figure 2 As shown, arrive They are the elevation values ​​of each pixel in the 3×3 window, in cm. is the size of the pixel. is 1 .

[0125] The design uses the third-order inverse distance weighted difference algorithm to calculate u and v, that is, and .

[0126] Step 3.8: Local variance of elevation gradient Calculation method: Using local variance Measures the rate of elevation change in a point cloud image within a local range. The larger the local variance average, the greater the local change on the surface of the point cloud data. Local variance calculation formula: .in, To analyze the elevation gradient in the x direction of the kth grid in the window, To analyze the elevation gradient in the y direction of the kth grid in the window, is the number of grids in the analysis window. In this scheme, the analysis window is 3×3, so . and It is the average value of the elevation gradient in the x and y directions of the 9 pixels in the analysis window.

[0127] Step 3.9: Local variance threshold of elevation gradient Calculation method:

[0128] Statistical analysis of local elevation gradient variance of a large number of known ground point clouds , take the mean and standard deviation .

[0129] set up ,in It is an empirical parameter (the value depends on the mapping environment, usually 2).

[0130] Step 3.10: Finally, combining the above two factors, we get the following final discriminant for binary classification: .

[0131] In the formula, Represents the final set of ground points. Represents the set of ground points in the nth smallest processing unit. Represents the union symbol, which means merging all sets that meet the conditions into one set. Indicates that for all minimum processing units Operation, where is the set of all minimum processing units. [·] represents Iverson brackets, where the condition in the brackets is 1 if it is true and 0 if it is false. The entire discriminant means: for all minimum processing units If the probability that the data point in the unit belongs to the ground point is greater than 0.5, the ground point set of the unit is Included in the final ground point set In this way, by setting parameters such as the number of iterations and thresholds, the final ground point set that integrates all the minimum processing unit information can be obtained. And the non-ground point set .like Figure 3 As shown, the red area is the detected ground point cloud.

[0132] In the laser SLAM mapping process, ground segmentation is an extremely important part. The existing algorithms only focus on the geometric features of laser point clouds, and there is a problem of blurred details at the junction. The present invention performs ground segmentation through innovative point cloud division methods: since ground points and non-ground points have different elevations, the ground seed points and the initial ground point set are first selected using the elevation difference. And because all ground points should have consistent connectivity with the seed points, they are further screened with the help of connectivity. In view of the problem that a very small number of ground seed points may be selected incorrectly, and non-ground points that also meet the connectivity, such as desktops and roofs, are mistakenly judged as ground points. The local elevation gradient is further used for re-judgment to screen out non-ground points with sudden changes in elevation gradients within a certain range. Thereby completing the refined segmentation of the ground in the mapping process.

[0133] Step 4: Finalize mapping and positioning

[0134] Step 4.1: Using the extracted feature points (including ground points and non-ground points ) The relative distance and position in the vehicle coordinate system are used to estimate the current position of the unmanned vehicle.

[0135] Step 4.2: Based on the estimated position, all point cloud data of the current frame are fused with the existing map. The fusion process includes adding new feature points and updating the positions of existing feature points.

[0136] Step 4.3: Repeat the above process, and after continuous fusion and optimization, a complete and accurate map is finally generated. Based on the relative positions of the feature points, the real-time position of the unmanned vehicle is accurately reflected in the map, achieving the goal of precise positioning.

[0137] Embodiment 2:

[0138] This embodiment introduces a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, a mapping and positioning method based on point cloud manifold analysis as described in any one of Embodiment 1 is implemented.

[0139] Embodiment 3:

[0140] This embodiment introduces a computer device, including:

[0141] Memory, used to store instructions.

[0142] A processor is used to execute the instructions so that the computer device performs the operations of a mapping and positioning method based on point cloud manifold analysis as described in any one of Example 1.

[0143] Embodiment 4:

[0144] This embodiment introduces an application example of the method of the present invention and a comparison experiment between the method of the present invention and the prior art to verify the accuracy and robustness of the positioning and mapping algorithm of the present solution.

[0145] The Velodyne VLP-16 laser radar model is used, and its parameters are: measurement range 0.1-150 meters, horizontal field of view angle 360°, vertical field of view angle 30°, vertical resolution 2°, and point frequency 300,000 points / second.

[0146] The laser radar is installed in the center of the roof, 1.0 meter above the ground, horizontally, and covers a 360° area. Its installation position is based on the geometric center of the vehicle to ensure that the point cloud data can fully cover the vehicle's surroundings.

[0147] This scheme is compared with the classic laser SLAM mapping algorithms LeGO-LOAM and LIO-SAM. Real data from multiple scenes such as cities, villages, national roads, and highways are collected for experiments. Figure 4The demonstration is a verification experiment conducted using a city scene as an example. The scene contains a large number of buildings, pedestrians and cars, as well as many sharp bends and straight roads, which can intuitively demonstrate the overall and local mapping effects of each algorithm. The verification of algorithm stability has extremely high reliability. Obviously, the algorithm proposed in the present invention has more complete details than the other two algorithms, the point cloud at the edge of the details is thinner, and there is no ghosting.

[0148] To further study the mapping effects of each algorithm, the three algorithms were compared with the ground truth under the same experimental conditions, and the algorithm performance was evaluated by the indicators Max maximum value, Min minimum value, Std standard deviation, Median median, Mean average value, and RMSE root mean square error. The absolute trajectory error comparison and Figure 5 Histogram of absolute pose errors.

[0149] The trajectory root mean square error of the algorithm of the solution of the present invention is 2.40m, which is 38.3% higher than the 3.89m of the LIO-SAM (Lightweight Inertial Odometry and SLAM) algorithm and 5.96m of the LeGO-LOAM (Lightweight and Ground-Optimized Lidar Odometry and Mapping) algorithm, and the positioning accuracy is improved by 59.7%. This shows that the algorithm accuracy of the solution of the present invention is much higher than that of the classic laser SLAM algorithm.

[0150] Table 1 Comparison of absolute trajectory errors of various algorithms

[0151]

[0152] The mapping algorithm in the solution of the present invention was deployed to the vehicle-mounted industrial computer for testing. In addition to conventional urban and rural environments, it was also tested in extreme lighting conditions (such as nighttime, strong backlight), high-dynamic environments (such as dense crowds, frequently moving obstacles), and cross-scenario generalization capabilities (such as transitioning from outdoor to indoor). The test results show that the algorithm can accurately map and locate in various environments, and the average processing speed can reach 62.7 frames per second.

[0153] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A mapping and positioning method based on point cloud manifold analysis, characterized in that: Specifically include: Obtain point cloud data of the environment, convert the point cloud data into a coordinate system, and obtain point cloud data in a vehicle coordinate system; The factor optimization objective function is constructed using the point cloud data in the vehicle coordinate system, the factor optimization objective function is solved, and the abnormal point cloud caused by the interference of dynamic objects and high-brightness mirror reflection objects is deleted to obtain the point cloud data after the abnormal points are deleted; A ground segmentation binary classification model is constructed using the point cloud data after outliers are deleted, and the ground segmentation binary classification model is solved to obtain an optimized ground point set and a non-ground point set; The optimized ground point set and non-ground point set are used to estimate the current position of the vehicle and generate a map. The factor optimization objective function expression is as follows: in, represents the factor optimization objective function, ∝ represents proportional to, ∏ represents the product, P(z dynamic |X,L) represents the dynamic factor, P(I k |M,I env ) represents the reflection intensity factor, X represents the position of the vehicle at each moment in the constructed image, L represents the point cloud data after the abnormal points are deleted, Z represents the point cloud data after registration, and z dynamic Represents the observed dynamic point cloud position data, I k represents the observed laser reflection intensity, M represents the material reflection characteristic matrix of objects in the environment, and I env Indicates the ambient light brightness; The solution factor optimizes the objective function, deletes abnormal point clouds caused by interference from dynamic objects and high-brightness mirror reflection objects, and obtains point cloud data after the abnormal points are deleted, specifically including: Factoring the optimization objective function Maximization is transformed into minimization of the energy function E(X,L); A nonlinear optimization algorithm is used to minimize the energy function E(X,L). During the minimization process: If a point has a dynamic factor residual r in consecutive frames dynamic If it is greater than the dynamic factor threshold, the point is considered as a dynamic point and removed from the point cloud; If the residual reflection intensity at a point If it is greater than the reflection intensity factor threshold, the point is considered as a high-brightness reflection noise point and removed from the point cloud; During the iterative optimization process, dynamic points and high-brightness reflection noise points are continuously removed until the energy function E converges or reaches the predetermined number of iterations; The final set of iteratively optimized vehicle positions X and static point cloud positions L are regarded as the optimal point cloud dataset.

2. The mapping and positioning method based on point cloud manifold analysis according to claim 1, characterized in that: The energy function E(X,L) is expressed as follows: Among them, w represents the weight, ρ(·) is the robust kernel function, and r dynamic represents the dynamic factor residual, Represents the reflected intensity residual.

3. The mapping and positioning method based on point cloud manifold analysis according to claim 1, characterized in that: The ground segmentation binary classification model expression is as follows: Among them, L(θ|χ) represents the likelihood function of the point cloud ground point, f(χ|θ) represents the conditional probability density function, ′ represents the product, and χ n The random variable representing the set of ground points in the nth smallest processing unit, θ n Represents the relevant parameters of the ground point set in the nth minimum processing unit.

4. The mapping and positioning method based on point cloud manifold analysis according to claim 3, characterized in that: The method of solving the ground segmentation binary classification model to obtain an optimized ground point set and a non-ground point set specifically includes: Get the initial value of the seed ground point selected by each minimum processing unit Set the initial value of the current ground plane to It is continuously updated during the iteration process. After m iterations, the final ground point set is selected. The final ground point collection Substitute into the ground segmentation binary classification model and solve f(χ n |θ n ), according to the final discriminant of the two categories Obtaining an optimized ground point set and a non-ground point set; Where G represents the ground point set, n represents the minimum processing unit, N represents the set of minimum processing units, ∪ represents the union symbol, [·] represents the Iverson bracket, G n Represents the set of ground points in the nth smallest processing unit.

5. The mapping and positioning method based on point cloud manifold analysis according to claim 4, characterized in that: The f(x n |θ n ) is expressed as follows: f(x n |θ n )=φ(p i )·ψ[φ(p i )]; Among them, φ(p i ) represents the connectivity element of the i-th point, ψ[φ(p i )] represents the elevation gradient element of the i-th point.

6. The mapping and positioning method based on point cloud manifold analysis according to claim 5, characterized in that: The connectivity factor φ(p i ) is expressed as follows: Among them, Conn(p i ) is point p i The connectivity metric value, Conn th It is an empirical threshold; Where N is the number of points in the point cloud corresponding to point p i The number of neighboring points whose distance in the horizontal direction is less than the threshold d; ‖*‖2 represents the norm of 2; exp represents the natural exponential function, n i Represents point p i The normal vector, n j Represents the neighboring point p j The normal vector of , σ is the smoothing parameter; The elevation gradient element ψ[φ(p i )]The expression is as follows: Among them, e represents a natural constant, G(p i ) represents point p i The local variance of the height gradient, G th is the threshold of the local variance of the height gradient.

7. A computer-readable storage medium, characterized in that: A computer program is stored thereon, and when the computer program is executed by a processor, a mapping and positioning method based on point cloud manifold analysis as described in any one of claims 1 to 6 is implemented.

8. A computer device, characterized in that: include: A memory for storing instructions; A processor is used to execute the instructions so that the computer device performs the operations of a mapping and positioning method based on point cloud manifold analysis as described in any one of claims 1 to 6.

Images