CN115512054B - Method for constructing three-dimensional space-time continuous point cloud map - Google Patents

Abstract

The invention discloses a method for constructing a three-dimensional space-time continuous point cloud map, which comprises the following steps: extracting laser characteristic points of the structured surface in the scene according to the laser radar data; according to the distribution characteristics of the feature points, an error ellipse model is constructed by utilizing a point-to-line measurement mode and a point-to-plane measurement mode, and reliable feature association between the feature points is screened out according to the error ellipse model; calculating optimal pose information according to the feature points extracted in the step 1 and the reliable feature association obtained in the step 2, and obtaining an initial conversion relation between a point cloud map coordinate system and a LiDAR reference coordinate system according to the optimal pose information; taking the initial conversion relation obtained in the step 3 as an initial value, and converting the space-time relation by using a spline model to realize optimal registration between LiDAR point clouds and a point cloud map; and performing domain searching on the registered laser points. The invention can more accurately reconstruct the three-dimensional point cloud map of the environment and can make up distortion information of laser point cloud data generated by movement.

Description

Construction method of three-dimensional spatiotemporal continuous point cloud map

Technical Field

The present invention belongs to the technical field of radar positioning and mapping, and specifically relates to a method for constructing a three-dimensional spatiotemporal continuous point cloud map.

Background technique

With the miniaturization and low cost of LiDAR equipment, and the diversified development of UAV and robot point cloud data acquisition platforms, the acquisition of high-precision three-dimensional laser point cloud data has become increasingly convenient and popular. Three-dimensional point cloud maps have gradually become an important form of expressing the real world, and its derivatives, such as high-precision maps, are playing an important role in emerging fields such as unmanned driving and new basic surveying and mapping. The core of laser point cloud data solution lies in high-precision real-time attitude positioning. SLAM (simultaneous localization and mapping) research with LiDAR as the main data source has received concentrated attention from academia and industry in recent years (Bisheng et al., 2017; Schwarz, 2010; Yang et al., 2015). At present, most SLAM research is mainly aimed at robot navigation and perception applications. The constructed point cloud maps (such as octree point cloud maps, surface element maps, etc.) are difficult to meet the needs of basic surveying and mapping and its extended applications in terms of time-space resolution and continuity.

LOAM (Zhang and Singh, 2014) established a laser SLAM algorithm framework based on feature association, and derived many variants such as A-LOAM, Lego-LOAM, and F-LOAM. This type of method uses the curvature properties of the laser scan line to extract corner feature points and plane feature points in the scene, and uses these features to solve the pose and map through independent threads. Lego-LOAM (Shan and Englot, 2018) obtains ground point cloud and object point cloud by pre-segmenting the original laser frame data, thereby improving the reliability of features. Deschaud (2018) improves the robustness of pose estimation by designing a metric model to screen robust feature points in the scene. In order to eliminate the non-rigid deformation caused by motion distortion in the point cloud, it is necessary to estimate the distortion using multiple frames of point clouds (Neuhaus et al., 2018). Dellenbach et al. (2021) simultaneously estimates the starting and ending poses of a frame of laser point cloud, and adds the assumption of speed change considering the continuity of motion. In order to reduce the delay caused by laser frame data, Qu et al. (2021) matched a frame of data in segments to achieve fast laser odometer. Inspired by the visual SLAM work (Whelan et al., 2015), Park et al. (2021, 2018) proposed a continuous-time 3D mapping method using facets for map fusion, and realized a map-centric dense mapping system by optimizing map details. On the other hand, Lovegrove et al. (2013) first proposed an incremental Lie group pose parameterization method based on splines and proposed a continuous-time SLAM theoretical framework (Droeschel and Behnke, 2018). Sheehan et al. (2013) used Catmull-Rom splines to fit the six degrees of freedom of the pose and realized a continuous-time positioning system. Lv et al. (2020) decoupled the translation part of the pose, used quaternions to model the rotation part, and combined with IMU data to achieve seamless calibration of IMU and LiDAR. Furgale et al. (2012) used the time basis function of splines to construct a set of optimization equations to obtain continuous time trajectory estimation, but lacked map optimization. Therefore, it is necessary to develop a method that can optimize map construction and generate high-precision cloud maps.

Summary of the invention

The purpose of the present invention is to address the deficiencies of the prior art and provide a method for constructing a three-dimensional spatiotemporal continuous point cloud map, which can more accurately reconstruct the three-dimensional point cloud map of the environment and compensate for the distortion information of the laser point cloud data caused by movement.

In order to solve the above technical problems, the present invention adopts the following technical solutions:

A method for constructing a three-dimensional spatiotemporal continuous point cloud map comprises the following steps:

Step 1, extracting laser feature points of structured surfaces in the scene based on the acquired laser radar data;

Step 2: According to the distribution characteristics of the feature points, an error ellipse model is constructed using point-to-line and point-to-surface measurement modes, and reliable feature associations between feature points are screened out based on the error ellipse model;

Step 3, calculating the optimal pose information of the feature points based on the feature points extracted in step 1 and the reliable feature association obtained in step 2, and obtaining the initial transformation relationship between the point cloud map coordinate system and the LiDAR reference coordinate system based on the optimal pose information;

Step 4, using the initial transformation relationship obtained in step 3 as the initial value, and using the spline model to model the continuous time motion of the feature points to achieve the optimal registration between the LiDAR point cloud and the point cloud map, thereby obtaining the precise registration parameters of any laser point;

Step 5: Perform a field search on the registered laser points to reduce the spatiotemporal redundancy of incremental point cloud map construction.

Furthermore, step 1 specifically includes:

A single frame of lidar data is divided into multiple scan lines according to the vertical angle of its laser point;

For each scan line, according to the distance value γ of the laser point and the horizontal angle resolution θ of the laser radar, Obtain the local distribution density of the point cloud near the laser point, so as to perform voxel downsampling on the point cloud;

After obtaining the scan line point cloud with similar spatial resolution, the single scan line point cloud is divided into blocks using the adaptive distance threshold ∈ to obtain the point cloud clustering result;

For each clustering result, the direction of the continuous vector is judged, the scattered points with fluctuating direction changes are filtered out, and the points with direction changes greater than 45° are re-clustered to obtain the edge feature point set and the plane point set to complete the extraction of the structured feature point cloud.

Furthermore, step 2 specifically includes:

According to the distribution characteristics of the extracted structured feature points, the point cloud neighborhood distribution information is used to find areas with strong linearity or planarity, so as to construct error vectors v _m , v _n and their corresponding weights w _m , w _n ;

Perform spatial clustering and SVD decomposition calculation on the error vectors v _m , v _n and their corresponding weights w _m , w _n to obtain their three-dimensional eigenvalues and corresponding eigenvectors;

An error ellipsoid is constructed with the obtained three-dimensional eigenvalue as the radius (r _x , _ry , r _z ) and the eigenvector as the radius direction V;

The parameters of the error ellipsoid are continuously updated iteratively so that the sphere has the optimal contribution value τ in all directions, and then reliable feature associations are found based on the contribution value τ.

Furthermore, the specific method of step 3 is:

For each frame of laser point cloud, first use the method in step 1 to extract the corresponding structured feature points And according to the pose obtained from the previous frame of laser point cloud Will Convert to the map coordinate system, and then select reliable feature associations according to the method in step 2, so as to construct an iterative least squares optimization equation and obtain the feature point by continuously optimizing the equation error. The optimal pose information of Finally, according to the optimal posture information Will Re-add it to the map coordinate system to obtain the initial transformation relationship between the point cloud map coordinate system and the LiDAR reference coordinate system, providing control information for subsequent LiDAR data.

Furthermore, the specific method of step 4 is:

Extract the structured feature point cloud within a certain time window, and obtain the initial estimated pose information of the feature point cloud according to the method in step 3. Use the spline model to fit the rotation and translation parts of the pose respectively, and obtain the corresponding pose information on the spline according to the timestamp of each laser point. Use the method in step 2 to construct the error term for the obtained pose information, optimize the error term to obtain the optimized pose information of each feature point cloud in the time window and align it to the cloud map.

Furthermore, step 5 is specifically as follows:

While performing map fusion and updating, the spatial neighborhood search method is used to perform reliability analysis on the point cloud within its neighborhood. If there are laser points in the area, only the points with the smallest uncertainty are retained. If not, the neighborhood information is directly updated.

Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention takes the unmanned vehicle laser radar data as the research object, formulates a structured feature extraction strategy according to its data characteristics, uses the error ellipse to screen reliable feature associations, and adopts a non-rigid point cloud fusion algorithm in continuous time, thereby completing the construction of a three-dimensional point cloud map of indoor and outdoor scenes of the unmanned aerial vehicle in a spatiotemporal manner; the method of the present invention can estimate the motion posture and position of a small unmanned vehicle in real time, and well compensates for the distortion information of the laser point cloud data caused by the motion, so that the three-dimensional point cloud map of the environment can be reconstructed more accurately, and the spatiotemporal continuity of the map points is guaranteed.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a flow chart of a method for constructing a three-dimensional spatiotemporal continuous point cloud map based on laser radar data according to an embodiment of the present invention;

FIG2 is a schematic diagram of structured feature point extraction according to an embodiment of the present invention;

FIG3 is a schematic diagram of structured feature point association screening based on error ellipse according to an embodiment of the present invention;

FIG4 is a schematic diagram of spline-based continuous-time non-rigid point cloud registration according to an embodiment of the present invention;

FIG5 is a schematic diagram of constructing a spatiotemporally continuous three-dimensional point cloud map according to an embodiment of the present invention.

Detailed ways

The technical solutions in the embodiments of the present invention will be described clearly and completely below in combination with the embodiments 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 ordinary technicians in this field without creative work are within the scope of protection of the present invention.

It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

The present invention will be further described below in conjunction with specific embodiments, but they are not intended to be limiting of the present invention.

As shown in FIG1 , the present invention discloses a method for constructing a three-dimensional spatiotemporal continuous point cloud map, comprising the following steps:

Step 1, extracting laser feature points of structured surfaces in the scene based on the acquired laser radar data;

This embodiment uses the unmanned vehicle laser radar data as the research object. For the obtained laser radar data, this step mainly uses four sub-steps to extract laser feature points: scan line extraction, scan line downsampling, scan line segmentation and incremental vector clustering, as shown in Figure 2. Specifically, first, a single frame of laser radar data is divided into multiple scan lines according to the vertical angle of its laser point. For example, the Velodyne16 laser radar can divide the original frame laser data into 16 scan lines; secondly, the same processing method is adopted for each scan line to obtain all the structured feature points in the scene. Since the distribution density of the point cloud has the characteristic of changing significantly with the distance from the laser radar, in order to obtain a scan line point cloud with relatively consistent spatial distribution, according to the distance value γ of the laser point and the horizontal angular resolution θ of the laser radar, The local distribution density of the point cloud near the laser point is obtained (Figure 2), so that the point cloud can be adaptively downsampled, that is, different voxel grid resolutions are adopted in different areas; after obtaining the scan line point cloud with similar spatial resolution, the adaptive distance threshold ∈ (Figure 2) is also used to divide the single scan line point cloud into blocks, so as to obtain the point cloud clustering result, for example, Pn _-1 , _{Pn are} two adjacent points on the same scan line, γn _-1 , _γn are the distances from the origin respectively, and Δθ is the angle between the two (Figure 2), is the limit angle value (usually 10°), and ∈ is calculated according to the following formula:

If the Euclidean distance between two points exceeds this value, that is, ‖Pn _-1 , _Pn‖ ＞∈, the two points are divided into blocks. If not, they are merged into the same class. Finally, for each clustering result, the direction of the continuous vector is judged, the scattered points with fluctuating direction changes are filtered out, and the points with large direction changes (>45°) are re-clustered, so that the edge feature point set and the plane point set can be obtained (Figure 2), completing the extraction of the structured feature point cloud.

Step 2: According to the distribution characteristics of the feature points, an error ellipse model is constructed using point-to-line and point-to-surface measurement modes, and reliable feature associations between feature points are screened out according to the error ellipse model;

In this step, according to the distribution characteristics of the extracted structured feature points, the point cloud neighborhood distribution information is used to find areas with strong linearity or planarity, so as to construct error vectors v _m , v _n and their corresponding weights w _m , w _n . The specific formula is as follows:

w _m = Φ(||P _⊥ -P _m ||)

w _n =Φ(|n ^T P _n +d|)

Among them, _Pm and _Pn are the feature points in the laser point cloud of the current frame, _P⊥ is the vertical point projected onto the fitted straight line of its neighborhood points, is the normal vector of the plane fitted to its domain point, is the plane equation, _Pi is a point on the line, Ф(*) represents the robust kernel function;

The error vector and its weight are spatially clustered and decomposed by SVD to obtain its three-dimensional eigenvalue and corresponding eigenvector, as shown in the following formula:

Where Ω represents the covariance matrix of the error vector, _wi represents the above weights, represents the corresponding error vector, U and V represent the result matrix of SVD decomposition, represents the error vector in the ellipsoid coordinate system;

The three-dimensional eigenvalue obtained by the above formula is the radius (r _x , _ry , r _z ) and the eigenvector is used as the radius direction V to construct the error ellipsoid. In this way, each error vector can be mapped to the ellipsoid coordinate system. The contribution value τ of the error vector to the pose optimization problem is estimated based on the distance d _i from the ellipsoid to the sphere (the radius of the sphere is the minimum contribution threshold) in the direction of the feature vector, as shown in the following formula:

In the formula, θ, represents the angle value of the vector in the polar coordinate system, x, y, z represent the coordinate values of the vector on the ellipsoid, R represents the desired sphere radius,

Reliable feature associations are found by sorting the contribution values τ of the error vectors (selecting the top 10 contribution values), and the parameters of the error ellipsoid (radius, direction of radius) are continuously updated iteratively, eventually achieving a high contribution value τ in all directions, as shown in Figure 3.

Step 3, calculating the optimal pose information of the feature point based on the feature point extracted in step 1 and the reliable feature association obtained in step 2, and obtaining the initial transformation relationship between the point cloud map coordinate system and the LiDAR reference coordinate system based on the optimal pose information;

In this step, for each frame of laser point cloud, the corresponding structured feature points are first extracted using the method in step 1. And according to the pose obtained from the previous frame of laser point cloud Will Convert to the map coordinate system, and then use the method in step 2 to select reliable feature associations, so as to construct an iterative least squares optimization equation, and obtain the feature point by continuously optimizing the error. The optimal pose information of Finally, according to the optimal posture information Re- Add it to the map coordinate system to obtain the initial transformation relationship between the point cloud map coordinate system and the LiDAR reference coordinate system, providing control information for the subsequent (step 4) LiDAR data;

In this step, the optimal estimation model of the pose is implemented by iterative least squares as follows:

Among them, f(T) is the nonlinear residual equation related to parameter T. To solve this equation, Taylor expand the objective function around T and retain only the first-order term:

in, is the first-order partial derivative of the error term with respect to the parameter T. Taking the derivative of the above equation and setting the derivative equal to zero gives the gradient direction:

The final variable to be solved is ΔT, so this is a system of linear equations, called incremental equations, and the equations are iteratively solved using the Gauss-Newton method or the Levenberg-Marquadt method until they converge to the optimal parameters.

Step 4, using the initial transformation relationship obtained in step 3 as the initial value, using the spline model to perform continuous time motion modeling, achieving optimal registration between the LiDAR point cloud and the point cloud map, thereby obtaining accurate registration parameters for any laser point;

In between these steps, the structured feature point cloud and its initial estimated pose information (from step 3) within a certain time window are extracted. Since the rotation and translation of the pose belong to different algebraic spaces, the spline model is used to fit the rotation and translation parts of the pose respectively, and the corresponding pose information on the spline is obtained according to the timestamp of each laser point. The pose information on the spline obtained is used to construct the error term generated in the registration process using the method in step 2, as shown in the following formula:

In the formula, represents the Nth pose in the time window, Ξ(*) is the error equation, Υ(*) is the spline fitting equation, Θ(η) is the timestamp of point p _η , is the pose corresponding to the point;

By minimizing the error term, the optimized pose information of each laser point in the time window is obtained and registered to the cloud map. In this way, the motion distortion in the data is eliminated while the point cloud is registered, thereby correcting the non-rigid deformation in the point cloud map and obtaining a continuous-time three-dimensional environment map, as shown in Figure 4.

Step 5: Perform a field search on the registered laser points to reduce the spatiotemporal redundancy of incremental point cloud map construction;

Since there are errors in the precise registration process of laser point clouds, considering the incremental update process of point cloud maps, the errors of point cloud fusion will gradually accumulate into the map. Therefore, it is necessary to perform a field search on the registered laser points to reduce the spatiotemporal redundancy of incremental point cloud map construction, as shown in Figure 5 below. The specific method is: while performing map fusion and update, the point cloud in its neighborhood is analyzed for reliability using the spatial neighborhood search method, as shown in Figure 5-(a). If there are laser points in the field, only the points with the smallest uncertainty are retained. If not, the neighborhood information is directly updated, as shown in Figure 5-(b). This method of updating point cloud maps improves the spatial continuity of three-dimensional mapping and reduces the spatial redundancy of point cloud maps.

The above are only preferred embodiments of the present invention, and are not intended to limit the implementation methods and protection scope of the present invention. Those skilled in the art should be aware that all solutions obtained by equivalent substitutions and obvious changes made using the contents of the specification of the present invention should be included in the protection scope of the present invention.

Claims (2)

1. The construction method of the three-dimensional space-time continuous point cloud map is characterized by comprising the following steps of:

Step 1, extracting laser characteristic points of a structured surface in a scene according to obtained laser radar data;

Step 2, constructing an error ellipse model by utilizing a point-to-line measurement mode and a point-to-surface measurement mode according to the distribution characteristics of the feature points, and screening out reliable feature association between the feature points according to the error ellipse model;

Step 3, calculating optimal pose information of the feature points according to the feature points extracted in the step 1 and the reliable feature association obtained in the step 2, and obtaining an initial conversion relation between a point cloud map coordinate system and a LiDAR reference coordinate system according to the optimal pose information;

Step 4, taking the initial conversion relation obtained in the step 3 as an initial value, and carrying out continuous time motion modeling on the characteristic points by utilizing a spline model so as to realize optimal registration between LiDAR point clouds and a point cloud map, thereby obtaining accurate registration parameters of any laser point;

step 5, carrying out neighborhood search on the registered laser points, and reducing the space-time redundancy of the incremental point cloud map construction;

The step 1 specifically includes:

Dividing single-frame laser radar data into a plurality of scanning lines according to the vertical angles of laser points of the single-frame laser radar data;

for each scan line, according to the distance value of the laser spot Horizontal angular resolution of lidarAccording toObtaining the local distribution density of the point cloud near the laser point, thereby carrying out voxel downsampling on the point cloud;

After obtaining a scan line point cloud with an approximate resolution, an adaptive distance threshold is utilized Partitioning the single scanning line point cloud to obtain a point cloud clustering result;

For each clustering result, carrying out direction discrimination of continuous vectors, filtering scattered points with fluctuation of direction change, and carrying out reclustering on points with direction change larger than 45 degrees, thereby obtaining an edge characteristic point set and a plane point set and completing extraction work of a structured characteristic point cloud;

The specific method of the step 3 is as follows:

for each frame of laser point cloud, extracting corresponding structured feature points by using the method of step 1 And according to the pose obtained by the laser point cloud of the previous frameWill beConverting into a map coordinate system, screening out reliable characteristic association according to the method of the step 2, thereby constructing an iterative least square optimization equation and obtaining the characteristic point by continuously optimizing equation errorsIs the optimal pose information of (a)Finally according to the optimal pose informationWill beAdding the initial conversion relation between the point cloud map coordinate system and the LiDAR reference coordinate system into the map coordinate system again, and providing control information for the follow-up laser radar data;

The specific method of the step 4 is as follows:

Extracting structured feature point clouds in a certain time window, acquiring pose information of initial estimation of the feature point clouds according to the method of the step 3, respectively fitting rotation and translation parts of the poses by adopting a spline model, acquiring corresponding pose information on the spline according to the time stamp of each laser point, constructing an error item for the acquired pose information by adopting the method of the step 2, optimizing the error item so as to acquire the pose information of each feature point cloud in the time window after optimization, and registering the pose information in a cloud map;

The step 5 is specifically as follows:

And (3) carrying out reliability analysis on the point cloud in the neighborhood by utilizing a space neighborhood searching method while carrying out map fusion and updating, if laser points exist in the neighborhood, only the point with the minimum uncertainty is reserved, and if the laser points do not exist, the neighborhood information is directly updated.

2. The method for constructing a three-dimensional space-time continuous point cloud map according to claim 1, wherein step 2 specifically comprises:

according to the distribution characteristics of the extracted structured feature points, searching a region with strong linearity or planarity by utilizing the point cloud neighborhood distribution information, thereby constructing an error vector Weight corresponding to the weight ；

For error vectorWeight corresponding to the weightCarrying out spatial clustering and SVD decomposition calculation to obtain a three-dimensional characteristic value and a corresponding characteristic vector;

Taking the obtained three-dimensional characteristic value as radius Constructing an error ellipsoid by taking the feature vector as a radial direction V;

Continuously and iteratively updating parameters of the error ellipsoid so that the sphere has optimal contribution values in all directions Then according to the size of the contribution valueTo find reliable feature associations.