Unmanned vehicle laser radar elevation constraint SLAM method fused with curvature fitting motion prediction

Abstract

The invention provides a low-line-number 3D laser radar elevation constraint SLAM method based on ground curvature fitting in order to solve the problems that a laser radar-inertial measurement unit SLAM system lacks effective constraint in the elevation direction and accumulative drifting is likely to be generated due to the fact that a low-line-number 3D laser radar is sparse in point cloud and insufficient in resolution in the vertical direction. The method comprises the following steps: firstly, carrying out preprocessing and ground point segmentation on point cloud data; then, in a vehicle real-time position neighborhood, quadric surface fitting is carried out on local ground points by using a least square method, ground normal vectors of the points are accurately extracted, and an average curvature representing the overall bending degree of the terrain is calculated; on the basis, normal vector constraint and average curvature correction are combined, and a new motion prediction and factor graph optimization model is constructed. Through the method, the observation capability of the low-line-number laser radar SLAM system in the elevation direction is remarkably enhanced, elevation drift is effectively inhibited, the positioning precision is improved, and compared with a mainstream open source algorithm, the average error is reduced by 46.3% to the maximum through actual measurement.

Description

TECHNICAL FIELD

[0001] The application relates to the field of unmanned vehicle system perception, in particular to the field of unmanned vehicle laser radar SLAM technology, and specifically to an unmanned vehicle laser radar elevation constraint SLAM method fusing curvature fitting motion prediction. BACKGROUND

[0002] With the rapid development of robot technology, 3D laser radar as an important environment perception sensor plays a crucial role in robot navigation, obstacle avoidance, and map construction. In modern unmanned vehicle positioning and mapping (SLAM) systems, laser radar (LiDAR) and inertial measurement units (IMU) are often used together to take full advantage of their complementary advantages in spatial perception and motion estimation. They are commonly used sensors in AGV and new energy vehicles. Laser radar is mainly responsible for providing high-precision spatial point cloud information, involving radar and supporting equipment manufacturing industry, while IMU can provide high-frequency acceleration and angular velocity data, involving intelligent perception and control equipment and other intelligent consumer device manufacturing industries. In theory, the combination of the two can improve the real-time performance and robustness of the system. Although significant progress has been made in SLAM algorithms based on laser radar and inertial measurement, there are still several problems that need to be further studied:

[0003] First, laser radar uses line scanning to perceive the environment. The more scanning lines the radar contains, the more information it can perceive. However, even if the current radar can contain more than 100 scanning lines, the perceived information is still sparse and costly. Using a low-line laser radar will further limit the information collected, and the accuracy and completeness of environmental perception will further decrease, which may affect the subsequent positioning and mapping effect; secondly, IMU calculates elevation by integrating acceleration data, but both low-precision and high-precision IMUs inevitably have bias and noise. These small system errors and random noise will continue to accumulate during integration, leading to cumulative elevation error. Finally, many existing SLAM systems assume that the ground is an ideal plane in the elevation constraint step, and after ground point extraction, plane fitting is directly performed. This assumption is more suitable for urban roads or artificially laid structured environments, but in unstructured road surfaces, the ground often has complex three-dimensional shapes such as undulations, convexities, concavities, and slope changes. Simple plane fitting cannot truly reflect the actual shape of the road surface, leading to distortion of the ground model and affecting the accuracy of elevation estimation.

[0004] To solve the above problems, a kind of unmanned vehicle laser radar elevation constraint SLAM method is proposed, which fuses curvature fitting motion prediction. The specific process includes: first, laser radar point cloud preprocessing is carried out, point cloud filtering and noise removal are carried out, then ground point cloud segmentation is carried out, and preliminary screening is carried out through the spatial distribution of ground point cloud;Secondly, the curvature and normal feature of local ground are extracted by using the quadratic surface fitting method, and the Z direction constraint obtained by curvature fitting is embedded into the optimization objective function of the SLAM system in the laser radar and IMU joint optimization framework, and the elevation estimation error is dynamically corrected;Finally, the historical ground curvature information and the current observation are fused in real time, and high robustness and high precision three-dimensional positioning are realized. This method can effectively compensate for the lack of observation in the vertical direction of low line number laser radar, significantly improve the elevation constraint ability in unstructured complex terrain environment, and enhance the longitudinal positioning accuracy and overall stability of the SLAM system.

[0005] The document "Laser inertial SLAM algorithm based on IESKF and graph optimization" proposes a method of combining error state Kalman filter and factor graph optimization to realize tight coupling of laser and IMU SLAM. Through global mapping and elevation correction, the elevation error is significantly reduced. This method mainly uses IESKF filtering and factor graph optimization to improve the overall accuracy, but does not deeply consider the microstructure of the ground, especially the adaptability to low line number laser and unstructured terrain.

[0006] The document "A cross-correction LiDAR SLAM method for high-accuracy 2D mapping of problematic scenario" proposes a cross-correction LiDAR SLAM method for high-accuracy 2D mapping of problematic scenario, which is aimed at SLAM cumulative error and abnormal scenarios such as loop failure and large loop error. Through data association and residual distribution optimization, the elevation and plane error are corrected, and the SLAM mapping accuracy is improved. This method focuses on global map optimization and error distribution adjustment in loop scenarios, and is weak in ground feature extraction and terrain adaptability. No special mechanism is proposed for the lack of elevation observation of low line number laser radar.

[0007] The document "Interactive 3D graph SLAM for map correction" proposes an interactive 3D graph SLAM method, which allows users to manually interact with the map and trajectory in the post-processing stage. By manually selecting key frames and ground pictures, the graph structure and spatial constraints are re-optimized to improve the accuracy and consistency of the final three-dimensional mapping. This method introduces human interaction, directly incorporating human experience into the SLAM optimization process, and focuses on improving the accuracy of post-processing, but has obvious shortcomings in real-time performance and adaptability SUMMARY

[0008] In view of the deficiencies of the prior art, the present application provides a low-line 3D laser radar height constraint SLAM method based on ground curvature fitting, which solves the problems raised in the above background art, and is characterized by comprising the following steps:

[0009] S1: point cloud data preprocessing: the collected original data is preprocessed and filtered to remove noise and interference, improve data quality, and normalize point cloud information to make it more comparable and analytical.

[0010] When removing noise from the collected original data in the S1: data preprocessing step, first determine the grid spacing based on the average point spacing of the point cloud data. Then establish a three-dimensional grid of the point cloud, then establish an outer space index grid based on the three-dimensional grid established by the point cloud, then count the number of points contained in each grid in the index grid, and then set an index structure window. Then traverse the index grid with the structure window as the basic unit, find the cubic grid containing only one data point, judge the discrete noise points according to the structure window index result, remove the data points judged as discrete noise points, re-count the number of points contained in the grid in the index grid, randomly select an index grid as a seed grid, perform diffusion operation, then remove the seed grid marked in the first diffusion operation, select a new seed grid for diffusion operation again, until all point grids are marked, finally count the number of data points involved in each diffusion operation, retain the point cloud containing the most points, and judge the other data points as clustered noise points and remove the clustered noise points.

[0011] The method for filtering the collected original data in the S1: data preprocessing step: identify the gross error points in the laser radar point cloud, and remove the gross error points. The commonly used criterion for gross error points is that the average distance between them and their neighborhood is greater than the sum of the global mean and the threshold. Then perform laser radar point cloud segmentation based on smooth surface growth to obtain objects, analyze the multi-echo proportion characteristics of the objects to identify potential ground objects, and remove the ground objects and the laser radar points contained therein, extract the feature points of the objects, and use the feature points to replace the original points contained in the objects to participate in subsequent operations, perform object category discrimination based on feature points, and update the category of the original laser radar points contained in the objects.

[0012] S2: ground point cloud segmentation:

[0013] Suppose a frame of laser point cloud data P is composed of N laser points p i , for each point p i of the laser point cloud, select a certain number of neighborhood points S i within the laser line centered on it, and calculate the curvature C i of the point:

[0014]

[0015] where ||r i || represents the Euclidean distance between point p i and the origin of the laser radar coordinate system, |S i | is the size of the neighborhood set, ||r j i || is the Euclidean distance between point p j and point p i .

[0016] According to the value of curvature C i , the flatness of the local surface where point i is located can be determined: if the curvature value is large, it represents that the point is at an uneven corner point; if the curvature value is small, it indicates that the point is on a flat local surface. After completing the curvature calculation of each frame, the points with occlusion or parallel relationship need to be marked, and then feature extraction is performed. When feature extraction, the point cloud of each frame is traversed in turn, and the data is divided into 6 regions, and a certain number of feature points are extracted in each region to ensure the uniformity of feature distribution. In each region, the points are sorted according to the curvature, and a curvature threshold k is set. When the curvature of the laser point is greater than k, it is determined as a corner point; when the curvature is less than k, it is considered as a plane point.

[0017] S3: Local ground quadratic surface fitting and curvature modeling:

[0018] S3.1: Local ground point extraction:

[0019] In the current position of the vehicle, the neighborhood ground points are selected within a set radius r to obtain a local ground point subset G local , which is represented as follows:

[0020]

[0021] where (x t , y t ) represents the projection coordinates of the estimated position of the vehicle, and r is selected as 10 meters to ensure the accuracy and real-time performance of local fitting.

[0022] S3.2: Least squares fitting of quadratic surface and curvature calculation:

[0023] The quadratic surface can well fit the elevation changes of general roads or local ground, and its parameters describe the curvature, inclination and undulation of the road surface and other spatial structure characteristics. All points (x, y, z) in the local ground point subset G local extracted in the above steps are fitted to the following quadratic surface by least squares method:

[0024] z = f(x, y) = ax 2 + by​2 + cxy + dx + ey + f (3)

[0025] The coefficients a, b, c, d, e, f are solved by linear least squares method by minimizing the height fitting residuals of all points. Specifically, a design matrix A and an observation vector Z are constructed:

[0026]

[0027] The optimal parameters are:

[0028] [a, b, c, d, e, f] T = (A T A) -1 A T Z (5)

[0029] For the surface fitted by the above steps, the principal curvatures are calculated at the projection point (x t , y t ) of the ground at the current position of the vehicle. The Hessian matrix of the quadratic surface is:

[0030]

[0031] The expression of the principal curvatures is:

[0032]

[0033] Assuming that k1 and k2 are the maximum and minimum principal curvatures respectively, then the average curvature k mean can be expressed as:

[0034]

[0035] The Gaussian curvature K can be expressed as:

[0036] K = k1 k2 = 4ab - c 2 (9)

[0037] In the above formula, the average curvature H represents the overall surface curvature, and a positive value represents a convex surface and a negative value represents a concave surface. The sign of the Gaussian curvature K determines the shape type of the surface: K > 0 represents an ellipsoid, K < 0 represents a saddle shape, and K = 0 represents a plane or a cylinder.

[0038] S4: Motion prediction model involving road surface curvature:

[0039] In high-precision unmanned vehicle positioning and mapping tasks, traditional motion prediction models are mainly based on the instantaneous state of the vehicle, including position, speed, heading angle and slope angle, etc., and predict the three-dimensional position of the vehicle at the next time through discrete time propagation, as follows:

[0040] After the ground quadric surface fitting parameter is determined, the normal vector of the ground can be determined at the fitting center point, the normal vector is the gradient direction of the surface at the point, which can be expressed as:

[0041]

[0042] In the formula,

[0043] Therefore, the above formula can be simplified as follows:

[0044]

[0045] The first-order partial derivative d and e are the rate of change of z with respect to x and y, reflecting the slope and tendency of the ground; the modulus of the normal vector is normalized and directly involved in the spatial constraint in motion prediction, so that the vehicle changes the elevation along the more real ground normal relationship; then the displacement of the unmanned vehicle within a specified time is solved, and the predicted position is orthogonal to the local ground normal vector to achieve accurate positioning of the unmanned vehicle in the elevation direction.

[0046] Let the current position of the vehicle be (x, y, z), the predicted position after motion be (x+Δx, y+Δy, z+Δz), and the local ground normal vector be n ground =(n x ,n y ,n z ). Then the vertical constraint can be expressed as:

[0047] n ground .[Δx,Δy,Δz] T =0 (12)

[0048] Solving the elevation increment Δz gives:

[0049]

[0050] The formula ensures that the vehicle displacement is always orthogonal to the ground normal vector, thereby preliminarily constraining the cumulative elevation error of the unmanned vehicle on the flat ground, but this method performs poorly on unstructured road surfaces, such as bumps, slopes, etc., and is difficult to constrain, so the invention adds a road curvature modified elevation prediction, using the ground curvature information obtained by quadric surface fitting to make a second-order correction to the elevation prediction, making the prediction result more in line with the actual complex terrain.

[0051] Assuming that the current position of the vehicle is the origin, and a small displacement d xy is made in any direction θ in the plane, then:

[0052]

[0053] Substituting Δx, Δy into the second-order term of the quadric surface gives:

[0054]

[0055] This second-order term is used to describe the effect of ground undulation on the actual elevation: in complex terrain, after the vehicle moves along the ground normal, only the first-order term will still produce errors, therefore, by adding a quadratic curvature correction, the change in elevation after the vehicle travels a small distance along the terrain can be more accurately predicted. This correction has a significant improvement effect on long-distance movement, gentle slope to steep slope, and continuous undulating areas.

[0056] Average curvature k mean It is a second-order terrain feature independent of direction, representing the overall bending trend of the ground surface at that point. In this invention, to simplify calculations and enhance robustness, the average curvature is used instead of each specific direction, that is, the second-order elevation increment in the vehicle's direction of travel is approximated by the average value:

[0057]

[0058] Therefore, taking into account all directions, the average value is:

[0059]

[0060] The final road surface curvature involved motion prediction model is obtained by combining the above plane and curvature correction:

[0061]

[0062] This model combines the normal fitting of the first-order term with the curvature correction of the second-order term, achieving both linear elevation changes for flat / sloping terrain and nonlinear predictions for terrain undulations, effectively avoiding cumulative elevation errors caused by terrain complexity and improving the elevation prediction accuracy of unmanned vehicles in complex scenarios. It is a high combination of physical constraints and engineering practicality.

[0063] S5: Factor graph SLAM optimization that fuses IMU pre-integration and ground curvature elevation constraints:

[0064] In a LiDAR-IMU SLAM system, the IMU can always collect data at a high frequency, for example, 200Hz, which is much higher than the common control frequency in current unmanned vehicle systems. At the same time, the LiDAR needs to continuously scan at different azimuth angles to cover a wider field of view and obtain high-precision spatial resolution, which directly leads to a reduction in the LiDAR scanning frame rate, for example, 10HZ. Due to the significant difference in sampling frequency between the IMU and the LiDAR, up to 20 times, the present application introduces additional segment constraints for the high-frequency local measurement data of the IMU between two LiDAR frames. Based on this, the present application designs a corresponding observation factor, taking the IMU local motion information as an additional constraint, and inserts it into the SLAM factor graph together with the ground curvature elevation constraint factor.

[0065] S5.1: Data preparation and timing alignment:

[0066] The original LiDAR point cloud sequence {P0, P1, …, P N} is obtained by each sensor carried on the unmanned vehicle, and the time stamp of each frame is t0, t1, …, t N ; the IMU raw data stream: three-axis acceleration {a k}, three-axis angular velocity {ω k} is obtained. The present application defaults that the data collection of the IMU and the LiDAR has been synchronized through time stamp alignment, so subsequent data only needs to be uniformly segmented in time sequence, without additional time synchronization steps.

[0067] S5.2: IMU pre-integration calculation and establishment of IMU local motion information factor:

[0068] The IMU local motion information factor is based on IMU pre-integration, which uses the high-frequency measurement of the IMU in a short time to predict the theoretical value of the next state variable through integration. Its residual term is defined as the difference between the actual optimization variable state and the integrated predicted state, including displacement, velocity and position, which reflects the consistency of the system state and the IMU physical observation.

[0069] Let the time of two frames of LiDAR be t i and t i+n , then the IMU segment is

[0070] S j =[t j ,t j+n ) (t i ≤t j <t j+n ≤t i+n ) (19)

[0071] Each Sj For one segment of IMU data, more detailed motion estimation is guaranteed. During this period, the integral result of the inertial measurement unit can be expressed as:

[0072]

[0073] where, represents the complete 15-dimensional integral state variable of the IMU. and respectively refer to the rotation, velocity and position changes from time t i to t j . b g,k is the gyro zero offset at the kth time, b a,k is the accelerometer zero offset at the kth time. Δt is the time interval between two IMU data frames, the integral result is used for the prediction part in the residual term, and the observation part comes from the reconstructed motion on the road surface.

[0074] In each iteration of the factor graph optimization, the residual is obtained by subtracting the current variable from the IMU pre-integral prediction result. The residual term is inserted into the factor graph as an IMU factor to participate in the overall optimization. The residual definition can be expressed as:

[0075]

[0076] S5.3: Establishment of ground curvature elevation constraint factor and observation factor:

[0077] Assuming that the three-dimensional position of the interpolation node k is p k = [x k , y k , z k ] T , and the local ground model is a quadratic surface, as shown in equation (6), then the elevation constraint residual is:

[0078] r ground,k = z k -f(x k , y k ) (22)

[0079] where z k is the actual elevation of the interpolation node k, and f(x k , y k ) is the elevation prediction value of the surface model at the point.

[0080] This factor constrains the vehicle to fit the actual elevation of the ground as much as possible at the interpolation node time, and the attitude to be as consistent as possible with the ground normal, to suppress the elevation drift and attitude error.

[0081] In summary, the observation factor objective function of the application contains IMU pre-integration factor and ground curvature elevation constraint factor, each observation factor is a residual term of observation error in state space, and the weighted square sum of all the residuals is minimized through optimization, so that the physical collaborative optimization of multi-source data is realized.

[0082]

[0083] In the formula, min x is the joint optimization of all variables. i is the weight of the i-th term.

[0084] 1. The ground point cloud segmentation method based on local surface curvature analysis proposed by the application is particularly suitable for low-line laser radar with sparse point cloud, significantly improves the accuracy and robustness of ground point extraction, and overcomes the problem that the traditional method is easily disturbed by noise and sparsity in ground segmentation of low-line radar data.

[0085] 2. The curvature modeling method based on local ground quadratic surface least squares fitting proposed by the application can accurately describe the spatial geometric characteristics of the road surface using sparse point cloud. The method dynamically extracts a local ground point subset in the neighborhood of the current position of the vehicle, robustly fits the quadratic surface equation by the least squares method, and calculates the key curvature parameters. These parameters quantitatively characterize the bending degree, inclination direction and surface type of the road surface, providing an accurate geometric model for low-line laser radar systems to understand complex local terrain and constrain the height movement of the vehicle, breaking through the limitations of the traditional plane hypothesis in sparse point cloud and unstructured terrain.

[0086] 3. The height prediction model fused with the average curvature correction of the road surface proposed by the application is a key innovation to solve the height cumulative error problem of low-line laser radar SLAM on unstructured road surface. Based on the traditional vertical constraint based on normal vector, the model innovatively introduces a second-order height correction term calculated by the average curvature. This correction term accurately quantifies the expected height change caused by road bending. This model significantly suppresses the Z-direction drift caused by insufficient elevation observation due to sparse point cloud, making the motion prediction on unstructured road surface more consistent with the actual terrain, providing reliable height constraints for low-line laser radar SLAM systems in complex terrain environments, and greatly improving the three-dimensional positioning accuracy and ground Figure One consistency.

[0087] 4、The application fuses the IMU pre-integration factor graph optimization and the ground curvature elevation constraint model, introduces the high-frequency motion information of the IMU and the ground structure information in the laser radar point cloud into the SLAM factor graph optimization framework. In the case that the low laser radar frame rate leads to the difficulty in accurately estimating the inter-frame motion, the high-frequency pre-integration factor of the IMU effectively makes up for the sparsity of the laser radar observation in space and time, and finely constrains the continuous motion trajectory of the vehicle between two laser radar frames. Meanwhile, the ground curvature elevation constraint factor further strengthens the cognition of the system to the actual road three-dimensional form. The joint optimization strategy of the IMU and the ground constraint not only effectively reduces the elevation cumulative error and Z-direction drift, but also greatly enhances the positioning robustness and ground consistency of the system in complex and unstructured terrain, thereby providing the low-line laser radar SLAM with all-around high-precision three-dimensional motion estimation capability. Figure One BRIEF DESCRIPTION OF DRAWINGS BRIEF DESCRIPTION OF DRAWINGS

[0088] In order to more clearly illustrate the technical solutions of the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiment or prior art description. Obviously, the drawings in the following description are only some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

[0089] Figure 1 Flow chart of the ground curvature fitting low-line 3D laser radar elevation constraint SLAM method;

[0090] Figure 2 Physical diagram of the unmanned vehicle and related sensors used in the experiment;

[0091] Figure 3 Point cloud schematic diagram of scene 1;

[0092] Figure 4 Trajectory schematic diagram of each algorithm in scene 1 compared with GPS data;

[0093] Figure 5 Schematic diagram of each axis trajectory error of each algorithm in scene 1 compared with GPS data;

[0094] Figure 6 Point cloud schematic diagram of scene 2;

[0095] Figure 7 Trajectory schematic diagram of each algorithm in scene 2 compared with GPS data;

[0096] Figure 8 Schematic diagram of each axis trajectory error of each algorithm in scene 2 compared with GPS data. DETAILED DESCRIPTION

[0097] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, a kind of unmanned vehicle laser radar elevation constraint SLAM method fusing curvature fitting motion prediction is provided, as shown in Figure 1 Including the following steps:

[0098] S1: point cloud data preprocessing: the original data collected is preprocessed and filtered, noise and interference are removed, data quality is improved, and point cloud information is normalized to make it more comparable and analytical.

[0099] When removing noise from the original data collected in the S1: data preprocessing step, first determine the grid spacing based on the average point spacing of the point cloud data. Then establish a three-dimensional grid of the point cloud, then establish an outer space index grid based on the three-dimensional grid established by the point cloud, then count the number of points contained in each grid in the index grid, and then set the index structure window. Then traverse the index grid with the structure window as the basic unit, find the cubic grid containing only one data point, judge the discrete noise points according to the structure window index result, remove the data points of the discrete noise points judged, re-count the number of points contained in the grid in the index grid, randomly select an index grid as a seed grid, perform diffusion operation, then remove the seed grid marked in the first diffusion operation, select a new seed grid for diffusion operation again, until all the point grids are marked, finally count the number of data points involved in each diffusion operation, retain the point cloud containing the most points, judge the other data points as clustered noise points, and remove the clustered noise points.

[0100] The method for filtering the original data collected in the S1: data preprocessing step: identify the gross error points in the laser radar point cloud, and remove the gross error points. The commonly used criterion for gross error points is that the average distance of its neighborhood is greater than the sum of the global mean and the threshold. Then perform laser radar point cloud segmentation based on smooth surface growth to obtain objects, analyze the multi-echo proportion characteristics of the objects to identify potential ground objects, and remove the ground objects and the laser radar points contained therein, extract the feature points of the objects, and use the feature points to replace the original points contained in the objects to participate in subsequent operations, perform object category discrimination based on feature points, and update the category of the original laser radar points contained in the objects.

[0101] S2: ground point cloud segmentation:

[0102] Suppose a frame of laser point cloud data P is composed of N laser points p i , for each point p i of the laser point cloud, select a certain number of neighborhood points S i within the laser line centered on it, and calculate the curvature C i of the point:

[0103]

[0104] where ||r i || represents the Euclidean distance between point p i and the origin of the laser radar coordinate system, |S i | is the size of the neighborhood set, ||r j i || is the Euclidean distance between point p j and point p i .

[0105] According to the value of curvature C i , the flatness of the local surface where point i is located can be determined: if the curvature value is large, it represents that the point is at an uneven corner point; if the curvature value is small, it indicates that the point is on a flat local surface. After completing the curvature calculation of each frame, the points with occlusion or parallel relationship need to be marked, and then feature extraction is performed. When feature extraction, the point cloud of each frame is traversed in turn, and the data is divided into 6 regions, and a certain number of feature points are extracted in each region to ensure the uniformity of feature distribution. In each region, the points are sorted according to the curvature, and a curvature threshold k is set. When the curvature of the laser point is greater than k, it is determined as a corner point; when the curvature is less than k, it is considered as a plane point.

[0106] S3: Local ground quadratic surface fitting and curvature modeling:

[0107] S3.1: Local ground point extraction:

[0108] In the current position of the vehicle, the neighborhood ground points are selected within a set radius r to obtain a local ground point subset G local , which is represented as follows:

[0109]

[0110] where (x t , y t ) represents the projection coordinates of the estimated position of the vehicle, and r is selected to be 10 meters to ensure the accuracy and real-time performance of local fitting.

[0111] S3.2: Least squares fitting of quadratic surface and curvature calculation:

[0112] The quadratic surface can well fit the elevation (z-axis direction) changes of the local ground of a general road, and its parameters describe the curvature, inclination and undulation of the road surface and other spatial structure characteristics. All points (x, y, z) in the local ground point subset G local extracted in the above step are fitted to the following quadratic surface by least squares method:

[0113] z = f(x, y) = ax 2 + by​2 + cxy + dx + ey + f (3)

[0114] The coefficients a, b, c, d, e, f are solved by linear least squares method by minimizing the height fitting residuals of all points. Specifically, a design matrix A and an observation vector Z are constructed:

[0115]

[0116] The optimal parameters are:

[0117] [a, b, c, d, e, f] T = (A T A) -1 A T Z (5)

[0118] For the surface fitted by the above steps, the principal curvatures are calculated at the projection point (x t , y t ) of the ground at the current position of the vehicle. The Hessian matrix of the quadratic surface is:

[0119]

[0120] The expression of the principal curvatures is:

[0121]

[0122] Assuming that k1 and k2 are the maximum and minimum principal curvatures respectively, then the average curvature k mean can be expressed as:

[0123]

[0124] The Gaussian curvature K can be expressed as:

[0125] K = k1 k2 = 4ab - c 2 (9)

[0126] In the above formula, the average curvature H represents the overall surface curvature, and a positive value represents a convex surface and a negative value represents a concave surface. The sign of the Gaussian curvature K determines the shape type of the surface: K > 0 represents an ellipsoid, K < 0 represents a saddle shape, and K = 0 represents a plane or a cylinder.

[0127] S4: Motion prediction model involving road surface curvature:

[0128] In high-precision unmanned vehicle positioning and mapping tasks, traditional motion prediction models are mainly based on the instantaneous state of the vehicle, including position, speed, heading angle and slope angle, etc., and predict the three-dimensional position of the vehicle at the next time through discrete time propagation, as follows:

[0129] After the ground quadric surface fitting parameters are determined, the normal vector of the ground can be determined at the fitting center point, which is the gradient direction of the surface at the point, and can be expressed as:

[0130]

[0131] In the formula,

[0132] Therefore, the above formula can be simplified as follows:

[0133]

[0134] The first-order partial derivatives d and e are the rate of change of z with respect to x and y, reflecting the slope and tendency of the ground; the modulus of the normal vector is normalized and directly involved in the spatial constraint in motion prediction, so that the vehicle changes in elevation along a more realistic ground normal relationship; then the displacement of the unmanned vehicle within a specified time is solved, and the predicted position is orthogonal to the local ground normal vector to achieve precise positioning of the unmanned vehicle in the elevation direction.

[0135] Let the current position of the vehicle be (x, y, z), the predicted position after motion be (x+Δx, y+Δy, z+Δz), and the local ground normal vector be n ground x y z Then the vertical constraint can be expressed as:

[0136] n ground .[Δx,Δy,Δz] T = 0 (12)

[0137] Solving the elevation increment Δz gives:

[0138]

[0139] This formula ensures that the vehicle displacement is always orthogonal to the ground normal vector, thereby preliminarily constraining the cumulative elevation error of the unmanned vehicle on flat ground, but this method performs poorly on unstructured road surfaces, such as bumps, slopes, etc., and is difficult to constrain, so the invention adds a road curvature modified elevation prediction, which uses the ground curvature information obtained by quadric surface fitting to perform a second-order correction on the elevation prediction, making the prediction result more consistent with the actual complex terrain.

[0140] Assuming that the current position of the vehicle is the origin and moving a small displacement d xy in any direction θ in the plane, then:

[0141]

[0142] Substituting Δx, Δy into the second-order term of the quadric surface gives:​​​

[0143]

[0144] This second-order term is used to describe the effect of ground undulation on the actual elevation: in complex terrain, after the vehicle moves along the ground normal, only the first-order term will still produce errors, therefore, by adding a quadratic curvature correction, the change in elevation after the vehicle travels a small distance along the terrain can be more accurately predicted. This correction has a significant improvement effect on long-distance movement, gentle slope to steep slope, and continuous undulating areas.

[0145] Average curvature k mean It is a second-order terrain feature independent of direction, representing the overall bending trend of the ground surface at that point. In this invention, to simplify the calculation and enhance the robustness, the average curvature is used instead of the specific direction, that is, the second-order elevation increment in the direction of vehicle travel is approximated by the average value:

[0146]

[0147] Therefore, taking into account all directions, the average value is:

[0148]

[0149] The final road surface curvature involved motion prediction model is obtained by combining the above plane and curvature correction:

[0150]

[0151] This model combines the normal fitting of the first-order term and the curvature correction of the second-order term, realizes linear elevation change for flat / slope terrain and nonlinear prediction for terrain undulation, effectively avoids the cumulative elevation error caused by terrain complexity, and improves the elevation prediction accuracy of unmanned vehicles in complex scenarios. It is a high combination of physical constraints and engineering practicality.

[0152] S5: Factor graph SLAM optimization that fuses IMU pre-integration and ground curvature elevation constraints:

[0153] In a LiDAR-IMU SLAM system, the IMU can always collect data at a high frequency, for example, 200Hz, which is much higher than the common control frequency in current unmanned vehicle systems. At the same time, the LiDAR needs to continuously scan at different azimuth angles to cover a wider field of view and obtain high-precision spatial resolution, which directly leads to a reduction in the LiDAR scanning frame rate, for example, 10HZ. Due to the significant difference in sampling frequency between the IMU and the LiDAR, up to 20 times, the present application introduces additional segment constraints for the high-frequency local measurement data of the IMU between two LiDAR frames. Based on this, the present application designs a corresponding observation factor, taking the IMU local motion information as an additional constraint, and inserts it into the SLAM factor graph together with the ground curvature elevation constraint factor.

[0154] S5.1: Data preparation and timing alignment:

[0155] The original LiDAR point cloud sequence {P0, P1, …, P N} is obtained by each sensor carried on the unmanned vehicle, and the time stamp of each frame is t0, t1, …, t N ; the IMU raw data stream: three-axis acceleration {a k}, three-axis angular velocity {ω k} is obtained. The present application defaults that the data collection of the IMU and the LiDAR has been synchronized through time stamp alignment, so subsequent data only needs to be uniformly segmented in time sequence, without additional time synchronization steps.

[0156] S5.2: IMU pre-integration calculation and establishment of IMU local motion information factor:

[0157] The IMU local motion information factor is based on IMU pre-integration, which uses the high-frequency measurement of the IMU in a short time to predict the theoretical value of the next state variable through integration. Its residual term is defined as the difference between the actual optimization variable state and the integrated predicted state, including displacement, velocity and position, which reflects the consistency of the system state and the IMU physical observation.

[0158] Let the time of two frames of LiDAR be t i and t i+n , then the IMU segment is

[0159] S j = [t j , t j+n ) (t i ≤ t j < t j+n ≤ t i+n ) (19)

[0160] Each Sj For one segment of IMU data, more detailed motion estimation is guaranteed. During this period, the integral result of the inertial measurement unit can be expressed as:

[0161]

[0162] where, represents the complete 15-dimensional integral state variable of the IMU. and respectively refer to the rotation, velocity and position changes from time t i to t j . b g,k is the gyro zero offset at the kth time, b a,k is the accelerometer zero offset at the kth time. Δt is the time interval between two IMU data frames, the integral result is used for the prediction part in the residual term, and the observation part comes from the reconstructed motion on the road surface.

[0163] In each iteration of the factor graph optimization, the residual is obtained by subtracting the current variable from the IMU pre-integral prediction result. The residual term is inserted into the factor graph as an IMU factor to participate in the overall optimization. The residual definition can be expressed as:

[0164]

[0165] S5.3: Establishment of ground curvature elevation constraint factor and observation factor:

[0166] Assuming that the three-dimensional position of the interpolation node k is p k = [x k , y k , z k ] T , and the local ground model is a quadratic surface, as shown in equation (6), then the elevation constraint residual is:

[0167] r ground,k = z k - f(x k , y k ) (22)

[0168] where z k is the actual elevation of the interpolation node k, and f(x k , y k ) is the elevation prediction value of the surface model at this point.

[0169] This factor constrains the vehicle to fit the actual elevation of the ground as much as possible at the interpolation node time, and the attitude to be consistent with the ground normal as much as possible, to suppress the elevation drift and attitude error.

[0170] In summary, the observation factor objective function of the application contains IMU pre-integration factor and ground curvature elevation constraint factor, each observation factor is a residual term of observation error in state space, and the weighted square sum of all these residuals is minimized through optimization, so as to realize physical collaborative optimization of multi-source data. The optimization objective is:

[0171]

[0172] In the formula, min x is the joint optimization of all variables. W i is the weight of the i-th term.

[0173] Embodiment: The experiment was carried out on a computer (RAM: 8.00 GB; processor: Intel(R) Core(TM) i5-9300H) of Ubuntu 18.04-Melodic version system. Figure 2 is the physical map of the unmanned vehicle used in this experiment. The unmanned vehicle is composed of an upper computer and a lower computer. The upper computer realizes the system building and software configuration of the unmanned vehicle. The lower computer controls the chassis and emergency stop button of the unmanned vehicle and other necessary hardware functions during the operation of the unmanned vehicle. The sensors carried by the unmanned vehicle include a 3D laser radar and an inertial odometer. The application uses a Speedtong Creative 16-line 3D laser radar RS-LIDAR-16, and the inertial odometer uses a Vite Intelligent IWT905.

[0174] The application is verified in combination with the test results as follows: the application uses two scenes to verify the algorithm, as shown in Figure 3 Figure 6 Figure 3 Scenario 1 is a standardized sports playground, and the unmanned vehicle runs for a short time and the ground is relatively regular. Figure 6 Scenario 2 is a long road section and an unstructured ground. There are pits and the like, steep slopes and the like in the road, so as to simulate the effectiveness and robustness of the application when the unmanned vehicle runs for a long time on complex road surfaces. Figure 4 Figure 7 is the trajectory diagram of each algorithm of the unmanned vehicle under each scene, wherein the dashed line represents the GPS data collected by the unmanned vehicle, and the GPS data is used as the true value reference of this embodiment. Figure 4 Figure 7 In the figure, the lines of different colors represent the corresponding trajectories of the unmanned vehicle in each scene of different algorithms, wherein the blue line represents the algorithm of the application, the green line represents the Lio-Sam algorithm, and the red line represents the Lego-Loam algorithm. Figure 5 Figure 8 is the trajectory error curve of the x-axis, y-axis and z-axis of the unmanned vehicle under each scene.

[0175] From Figure 4 ,​​​​​Figure 7 It can be seen that the trajectory of the unmanned vehicle of the algorithm in the present application is more consistent with the GPS data than the mainstream open source algorithm in various scenarios, and the elevation drift phenomenon is reduced. The specific elevation error values are shown in Table 1 and Table 2. According to Table 1, the average value of the elevation error of the algorithm in the present application is reduced by 38.5% compared with the Lio-Sam algorithm, the standard deviation is reduced by 40.8%, and the root mean square error is reduced by 32.2%; compared with the Lego-Loam algorithm, the average value is reduced by 72.12%, the standard deviation is reduced by 81.79%, and the root mean square error is reduced by 74.01%. According to Table 2, the average value of the elevation error of the algorithm in the present application is reduced by 41.7% compared with the Lio-Sam algorithm, the standard deviation is reduced by 58.6%, and the root mean square error is reduced by 54.8%; compared with the Lego-Loam algorithm, the average value is reduced by 46.3%, the standard deviation is reduced by 63.8%, and the root mean square error is reduced by 52.5%.

[0176] Finally, it should be noted that: the above specific implementation solutions further illustrate the purposes, technical solutions and beneficial effects of the present application. The above examples are only used to illustrate the technical solutions of the present application, and are not a limitation on the protection scope of the present application. Those skilled in the art should understand that any modification or equivalent replacement of the technical solutions of the present application is included in the protection scope of the present application.

[0177] Table 1 Error values of each algorithm in scene 1

[0178]

[0179] Table 2 Error values of each algorithm in scene 2

[0180]

Claims

1. An unmanned vehicle lidar elevation-constrained SLAM method fusing curvature-fitted motion prediction, characterized in that, Comprising the following steps: S1: Point cloud data preprocessing: The original data collected is preprocessed and filtered to remove noise and interference, improve data quality, and normalize point cloud information for better comparability and analysis; S2: Ground point cloud segmentation: Suppose a frame of laser point cloud data is P, which is composed of N laser points p i Each point p i In the laser point cloud, a certain number of neighborhood points S i are selected in the laser line centered on it, and the curvature C i of the point is calculated: wherein ||r i || represents the Euclidean distance between the point p i and the origin of the laser radar coordinate system; |S i is the size of the neighborhood set, wherein ||r j -r i || is the Euclidean distance between the point p j and the point p i . According to the curvature C i The value of curvature C can determine the flatness of the local surface where point i is located: if the curvature value is large, it represents that the point is at an uneven corner point; if the curvature value is small, it indicates that the point is on a flat local surface; after completing the curvature calculation of each frame, the points with occlusion or parallel relationship need to be marked, and then feature extraction is performed; during feature extraction, the point cloud of each frame is traversed in turn, and the data is divided into 6 regions, and a certain number of feature points are extracted in each region to ensure the uniformity of feature distribution; in each region, the points are sorted according to the curvature, and a curvature threshold k is set; when the curvature of the laser point is greater than k, it is determined as a corner point; when the curvature is less than k, it is considered as a plane point; S3: Local ground quadric surface fitting and curvature modeling: S3.1: Local ground point extraction: At the current position of the vehicle, ground points within a neighborhood are selected with a set radius r, resulting in a local subset of ground points G local is represented as follows: where (x t ,y t ) denotes the projected coordinates of the vehicle estimated position, r is chosen to be 10 meters to guarantee the accuracy and real-time of the local fitting. S3.2: Least squares fitting of quadric surface and curvature calculation: The quadric surface can well fit the elevation change of the general road or local ground, and its parameters describe the spatial structure characteristics of the road surface curvature, inclination and undulation; The subset of local ground points G extracted in the above step local All points (x, y, z) in the middle are fitted with a least squares method as follows: z = f(x, y) = ax 2 + by 2 + cxy + dx + ey + f (3) The coefficients a, b, c, d, e, f are solved by minimizing the height fitting residual of all points using linear least squares method; Specifically, a design matrix A and an observation vector Z are constructed: The optimal parameters are: [a, b, c, d, e, f] T = (A T A) -1 A T Z (5) The principal curvatures are calculated for the projection point (x t ,y t ) of the fitted surface at the current position of the vehicle on the ground; the Hessian matrix of the quadric surface is: The expression of principal curvature is: Assuming k1, k2 are the maximum and minimum principal curvatures, respectively, then the mean curvature k mean may be expressed as: The Gaussian curvature K can be expressed as: K = k1-k2= 4ab - c 2 (9) In the above formula, the average curvature H is the overall surface curvature, and the positive value is convex, and the negative value is concave; The sign of the Gaussian curvature K determines the surface shape type: K>0 is ellipsoidal, K<0 is saddle-shaped, and K=0 is a plane or cylinder; S4: Motion prediction model involving road surface curvature: In the high-precision unmanned vehicle positioning and mapping task, the traditional motion prediction model is mainly based on the instantaneous state of the vehicle, including position, speed, heading angle and slope angle, etc., which predicts the three-dimensional position of the vehicle at the next time through discrete time propagation, as follows: After determining the fitting parameters of the ground quadric surface, the normal vector of the ground can be determined at the fitting center point, which is the gradient direction of the surface at that point, and can be expressed as: Let the current position of the vehicle be (x, y, z), the predicted position after motion be (x + Δx, y + Δy, z + Δz), and the local ground normal be n ground = (n x ,n y ,n z ); then its vertical constraint can be expressed as: n ground .[Δx,Δy,Δz] T = 0 (11) Solving the height increment Δz: This formula ensures that the vehicle displacement is always orthogonal to the ground normal vector, thereby preliminarily constraining the height error accumulation of the unmanned vehicle on the flat ground, but this method performs poorly on unstructured roads, such as bumps, slopes, etc., and is difficult to constrain, so the invention adds a road curvature correction height prediction, which uses the ground curvature information obtained by quadric surface fitting to make a second-order correction to the height prediction, making the prediction result more consistent with the actual complex terrain; Assume the current position of the vehicle is the origin, and advance a small displacement d in an arbitrary direction θ in the plane xy Then we have: Substituting the above Δx, Δy into the second-order term of the quadric surface can obtain: This second-order term is used to describe the influence of ground undulation on the actual height: In complex terrain, after the vehicle moves along the ground normal, only the first-order term will still produce errors, therefore, by adding the second-order curvature correction, the change in height after the vehicle moves a small distance along the terrain can be predicted more finely; This correction has a significant improvement effect on long-distance movement, gradual slope to steep slope, and continuous undulating area; average curvature k mean is a direction-independent second-order topographic property representing the overall bending tendency of the surface at that point; in the present invention, to simplify the calculations and enhance robustness, instead of targeting each specific direction, the average curvature is employed, i.e. the second-order elevation increment in the direction of travel of the vehicle is assumed to be approximated by the average value: Therefore, considering all directions, the average is: The final motion prediction model involving road surface curvature is obtained by combining the above plane and curvature correction: S5: Factor graph SLAM optimization integrating IMU pre-integration and ground curvature height constraint: In a laser radar-inertial measurement unit (LiDAR-IMU) SLAM system, an inertial measurement unit (IMU) can always collect data at a high frequency, for example, 200 Hz, which is much higher than the common control frequency in current unmanned vehicle systems; at the same time, the laser radar needs to continuously scan at different azimuth angles to cover a wider field of view and obtain high-precision spatial resolution, and this working mode directly leads to the reduction of the laser radar scanning frame rate, for example, 10 Hz; due to the significant difference of up to 20 times between the sampling frequencies of the IMU and the laser radar, the application introduces an additional segmented constraint for the high-frequency local measurement data of the IMU between two laser radar frames; based on this, the application designs a corresponding observation factor, takes the IMU local motion information as an additional constraint, and inserts it into the SLAM factor graph together with the ground curvature elevation constraint factor; S5.1: Data preparation and timing alignment: The original laser radar point cloud sequence {P0, P1, …, P N} is acquired by each sensor carried on the unmanned vehicle, the time stamp of each frame is t0, t1, …, t N ; the IMU original data stream: three-axis acceleration {a k}, three-axis angular velocity {ω k} is acquired; the data collection of the IMU and the laser radar is synchronized by default through the time stamp alignment, so that subsequent uniform segmentation of the data in time sequence is required, and no additional time synchronization step is required; S5.2: IMU pre-integration calculation and establishment of IMU local motion information factor: During this period, the integral result of the inertial measurement unit can be represented as: where denotes the complete 15-dimensional integrated state variable of the IMU; and denote the rotation, velocity and position changes from time t i to t j ; b g,k is the gyro bias at the kth time instant, b a,k is the accelerometer bias at the kth time instant; Δt is the time interval between two IMU data frames, the integration results are used in the prediction part of the residual term, while the observation part is derived from the reconstructed motion on the road surface; In each iteration of the factor graph optimization, the residual is obtained by subtracting the current variable from the IMU pre-integration prediction result; the residual term is inserted into the factor graph as an IMU factor to participate in the overall optimization; the residual definition can be represented as: S5.3: Establishment of ground curvature elevation constraint factor and observation factor: Assume the three-dimensional position of the interpolation node k is p k = [x k , y k , z k ] T The elevation constraint residual is as shown in formula (6) when the local ground model is a quadratic surface: r ground,k = z k -f(x k , y k ) (20) where z k is the actual elevation of the interpolation node k, f(x k ,y k ) is the elevation prediction value of the surface at this point; This factor constrains the vehicle to fit the actual elevation of the ground as much as possible when interpolating the node time, and the attitude to be consistent with the ground normal as much as possible, so as to suppress the elevation drift and attitude error; In summary, the observation factor objective function of the application contains the IMU pre-integration factor and the ground curvature elevation constraint factor; each observation factor is a residual term of an observation error in the state space, and through optimization, the weighted sum of all these residuals is minimized, so as to realize the physical collaborative optimization of multi-source data; the optimization objective is: min x is a joint optimization of all variables; W i is the weight of the ith term.

Images