Map construction method and system based on continuous time and curved surface features

Abstract

The embodiment of the invention provides a map construction method and system based on continuous time and curved surface features, and the method comprises the steps: removing motion distortion through point cloud preprocessing, querying a neighbor point set from a local point cloud map, and fitting a quadratic polynomial curved surface; a continuous time pose concept is fused, poses at the beginning and ending moments of a frame of point cloud are used as variables to construct a point-to-curved surface residual error, an optimization problem is designed, and a Gaussian Newton method is adopted to iteratively solve an optimal pose; meanwhile, by updating the local map and cutting voxels exceeding a set range, the high efficiency of the map structure is maintained; the method makes full use of curved surface features such as common trees and telegraph poles in an automatic driving scene, further gets rid of dependence on a plane structured environment and an inertial measurement unit by means of continuous-time high-frequency estimation, improves the adaptability of the laser odometer to a complex environment, effectively solves the problem of insufficient constraint caused by insufficient plane features, and improves the accuracy of the laser odometer. And the mapping precision and stability are obviously improved.

Description

Technical Field

[0001] Embodiments of the present invention relate to the field of navigation and positioning technology, and in particular to a map construction method, system, electronic device, and storage medium based on continuous time and surface features. Background Art

[0002] Amid the rapid development of autonomous driving technology, map construction is a core foundation. Current mainstream solutions primarily use LiDAR (LiDAR) to fuse sensor data from inertial measurement units (IMUs) and global positioning systems (GPS) to construct three-dimensional maps of the environment. This technology typically assumes that the environment contains numerous planar features. By extracting these features and constructing a point-to-plane residual optimization problem, it achieves pose estimation and map assembly.

[0003] Existing mapping frameworks are characterized by a high reliance on planar features. For example, in structured road environments, planar features such as walls and ground surfaces are easily extracted, allowing for stable pose constraints to be achieved through plane fitting and residual optimization. Furthermore, some improved methods optimize plane estimation accuracy by performing uncertainty analysis on planar features, or by analyzing information matrices to identify environmental degradation and modify estimates of specific degrees of freedom.

[0004] Dependence on planar features leads to insufficient scene adaptability. When the number of planar features in the environment decreases (such as in autonomous driving scenarios with a large number of curved objects such as trees and telephone poles), the number of effective planar feature extractions drops significantly, and the effective constraints on pose estimation are insufficient, which in turn causes map drift or even map construction failure.

[0005] Although some methods improve the accuracy of plane estimation through uncertainty analysis, they still cannot get rid of the dependence on planar structured environments; and the effectiveness of environmental degradation identification methods based on information matrices depends on the sufficiency and correctness of non-degenerate dimensional constraints. When the overall number of relevant features in the scene is insufficient, the method will fail due to the lack of reliable constraints.

[0006] Traditional mapping frameworks typically rely on inertial measurement units to provide high-frequency pose estimation to compensate for the lack of motion information between lidar frames. However, this increases the system's dependence on and cost of hardware, and affects mapping accuracy when inertial device errors accumulate. Summary of the Invention

[0007] The embodiments of the present invention provide a map construction method and system based on continuous time and surface features to solve the problem in existing technologies that map construction relies on plane assumptions, resulting in limited mapping performance and the inability to completely get rid of the dependence on planar structured environments.

[0008] In a first aspect, an embodiment of the present invention provides a map construction method based on continuous time and surface features, comprising:

[0009] S1. Determine the world pose of the lidar at the start and end times of the point cloud data of the current frame, determine the continuous-time pose based on the world pose of the lidar at the start and end times, and convert the point cloud data of the current frame from the lidar coordinate system to the world coordinate system using the continuous-time pose; wherein the point cloud data is a collection of environmental point clouds collected by the lidar within one scanning cycle;

[0010] S2. Registering the point cloud data of the current frame to the constructed local point cloud map, querying the neighboring point set of the environment point cloud in the point cloud data, constructing a local tangent space coordinate system based on the neighboring point set, converting the coordinates of the neighboring point set to the local tangent space coordinate system, constructing an overdetermined system of equations, and fitting to obtain quadratic polynomial surface coefficients;

[0011] S3, construct the residual of the environment point cloud to the quadratic polynomial surface, and determine the continuous time pose with the minimum residual;

[0012] S4. Repeat steps S1 to S3 until the preset number of iterations is reached or the change in the continuous-time pose is less than the set threshold, and the optimized continuous-time pose is obtained. According to the optimized continuous-time pose, the point cloud data of the current frame is converted from the lidar coordinate system to the world coordinate system to obtain the final global point cloud map.

[0013] Preferably, the step S1 specifically includes:

[0014] S11, after obtaining a frame of point cloud data collected by the laser radar, downsample the original point cloud through voxel filtering, and assign the environment point cloud to the corresponding voxel through a three-dimensional spatial hash table, retaining only the centroid point in each voxel as the representative point;

[0015] S12. Obtain the world pose of the laser radar at the start and end moments of the point cloud data, and construct a continuous-time pose based on the world pose of the laser radar at the start and end moments; for each environmental point cloud in a frame of point cloud data, determine the instantaneous pose of the laser radar at the acquisition moment by linear interpolation and smooth spherical linear interpolation methods according to the relative position of the environmental point cloud at the acquisition moment within the scanning cycle, and convert the corresponding environmental point cloud from the laser radar coordinate system to the world coordinate system based on the instantaneous pose of the laser radar.

[0016] Preferably, in step S2, registering the point cloud data of the current frame to the constructed local point cloud map, and querying the neighboring point set of the environment point cloud in the point cloud data specifically includes:

[0017] Registering the point cloud data of the current frame to the constructed local point cloud map. If the point cloud data of the current frame is the first frame of point cloud data, it is directly registered to the local point cloud map. Otherwise, aligning the point cloud data of the current frame with the historical point cloud data in the local point cloud map to obtain the precise pose of the point cloud data of the current frame in the world coordinate system, and then registering the point cloud data of the current frame to the local point cloud map; wherein the local point cloud map is a point cloud map with a hash voxel structure;

[0018] For the environmental point cloud in each frame of point cloud data, the spatial index of the hash voxel and the eight-neighborhood search method are used to count the neighboring environmental point clouds within the eight voxels around the environmental point cloud to form a neighbor point set.

[0019] Preferably, in step S2, a local tangent space coordinate system is constructed based on the neighboring point set, the coordinates of the neighboring point set are converted to the local tangent space coordinate system, an overdetermined set of equations is constructed, and a surface is obtained by fitting, specifically including:

[0020] Calculate the centroid and covariance matrix of the neighboring point set, take the eigenvector corresponding to the minimum eigenvalue as the Z axis; select the eigenvector perpendicular to the Z axis and with the largest eigenvalue as the X axis, and obtain the Y axis by the cross product of the Z axis and the X axis. Make the three axes orthogonal to each other and normalize them to form a right-handed coordinate system;

[0021] Taking the nearest neighbor point to the current environment point cloud in the neighbor point set as the origin, using the orthogonal axes formed by the X-axis, Y-axis, and Z-axis to form a transformation matrix, the neighbor point in the world coordinate system is transformed into the local tangent space coordinate system based on the transformation matrix;

[0022] According to the coordinates of the neighboring point set in the local tangent space coordinate system, an overdetermined system of equations about the coefficient vectors of the quadratic polynomial surface is constructed, and the overdetermined system of equations is solved by the least squares method to obtain the local quadratic polynomial surface coefficients.

[0023] Preferably, in step S2, the equation of the quadratic polynomial surface is:

[0024] f(x,y)=[a][x 2 ,y 2 ,xy,x,y]

[0025] [a]=[a1,a2,a3,a4,a5]

[0026] In the above formula, f(x,y) is the equation of the quadratic polynomial surface fitted in the local tangent space; x, y, and z represent the x-axis, y-axis, and z-axis coordinates of the neighboring points in the local tangent space coordinate system, respectively; [a] represents the coefficient vector of the quadratic polynomial surface, and a1, a2, a3, a4, and a5 represent the x-axis in the equation of the quadratic polynomial surface. 2 、y2 , xy, coefficients of x, y terms;

[0027] The overdetermined system of equations for the coefficient vector [a] is:

[0028]

[0029] In the above formula, (x1,y1,z1),(x2,y2,z2),...,(x n ,y n ,z n ) represents the coordinates of each neighbor point in the neighbor point set in the local tangent space coordinate system.

[0030] Preferably, the step S3 specifically includes:

[0031] For the environment point cloud in the current frame point cloud data:

[0032]

[0033] Build the residual from the environment point cloud to a quadratic polynomial surface:

[0034]

[0035] In the above formula, e i [X] is the residual of the i-th environment point cloud; represents the three-dimensional coordinates of the i-th environmental point cloud in the laser radar coordinate system in the current frame point cloud data; X = [T a ,T e ] represents a frame of point cloud data at the starting time T a and the end time T e The position of the lidar in the world coordinate system at this time; It is the coordinate of the environment point cloud after being converted to the local tangent space coordinate system; is the predicted value of the local quadratic polynomial surface in the tangent space;

[0036] Based on X, the current frame point cloud data is converted from the lidar coordinate system to the world coordinate system, and then to the local tangent space coordinate system:

[0037]

[0038] In the above formula, is the laser radar coordinate system at τ i The pose in the world coordinate system at the moment, τ i is the collection time of the i-th environment point cloud; Indicates the i-th environment point cloud in the current frame point cloud data, after The three-dimensional coordinates in the local tangent space coordinate system after transformation; is the transformation matrix of the local tangent space coordinate system; is the coordinate of the i-th environment point cloud in the current frame point cloud data in the world coordinate system; is the center point of the neighboring point set; For the rotating part, is the translation part;

[0039] Determine the optimization objective function of the current frame point cloud data:

[0040]

[0041] In the above formula, I n is the number of valid points involved in residual calculation; β is the loss function; is the square of the residual of the i-th environment point cloud;

[0042] Determine the continuous time pose X with the minimum residual a ,T e ].

[0043] Preferably, in step S4, after converting the point cloud data of the current frame from the lidar coordinate system to the world coordinate system according to the optimized continuous-time pose, the method further includes:

[0044] Update the local point cloud map, register the point cloud data to the local point cloud map, traverse the local point cloud map, determine the distance between the map voxel and the corresponding lidar pose, and delete the corresponding map voxel if the determined distance exceeds a preset distance threshold.

[0045] In a second aspect, an embodiment of the present invention provides a map construction system based on continuous time and surface features, including:

[0046] a point cloud preprocessing module for determining the world pose of the lidar at the start and end times of the point cloud data of the current frame, determining a continuous-time pose based on the world pose of the lidar at the start and end times, and converting the point cloud data of the current frame from the lidar coordinate system to the world coordinate system using the continuous-time pose; wherein the point cloud data is a collection of environmental point clouds collected by the lidar within one scanning cycle;

[0047] a surface fitting module that registers the point cloud data of the current frame to a constructed local point cloud map, queries the neighboring point set of the environment point cloud in the point cloud data, constructs a local tangent space coordinate system based on the neighboring point set, transforms the coordinates of the neighboring point set to the local tangent space coordinate system, constructs an overdetermined system of equations, and fits the quadratic polynomial surface coefficients;

[0048] The residual optimization module constructs the residual from the environment point cloud to the quadratic polynomial surface and determines the continuous time pose with the minimum residual;

[0049] The map update module repeats the steps from the point cloud preprocessing module to the residual optimization module until the preset number of iterations is reached or the change in the continuous-time pose is less than the set threshold, and obtains the optimized continuous-time pose. According to the optimized continuous-time pose, the point cloud data of the current frame is converted from the lidar coordinate system to the world coordinate system to obtain the final global point cloud map.

[0050] In a third aspect, an embodiment of the present invention provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps of the map construction method based on continuous time and surface features as described in the embodiment of the first aspect of the present invention are implemented.

[0051] In a fourth aspect, an embodiment of the present invention provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps of the map construction method based on continuous time and surface features as described in the embodiment of the first aspect of the present invention are implemented.

[0052] The embodiments of the present invention provide a map construction method, system, electronic device and storage medium based on continuous time and surface features. The method removes motion distortion through point cloud preprocessing, queries a set of neighboring points from a local point cloud map and fits a quadratic polynomial surface. The method integrates the concept of continuous time pose, constructs the point-to-surface residual with the pose at the start and end of a frame of point cloud as variables, designs an optimization problem, and uses the Gauss-Newton method to iteratively solve the optimal pose. The method also maintains map efficiency by updating the local map and cropping voxels that exceed the set range. The method makes full use of surface features such as trees and utility poles commonly found in autonomous driving scenarios, gets rid of excessive reliance on planar structured environments and inertial measurement units, improves the adaptability of laser odometers to complex environments, effectively solves the problem of insufficient constraints caused by insufficient planar features, and significantly improves the accuracy and stability of map construction. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following is a brief introduction to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0054] Figure 1 is a flowchart of a map construction method based on continuous time and surface features according to an embodiment of the present invention;

[0055] Figure 2 It is a specific flow chart of step S1 according to an embodiment of the present invention.

[0056] Figure 3 is a block diagram of a map construction system based on continuous time and surface features according to an embodiment of the present invention;

[0057] Figure 4 Schematic diagram of the physical structure according to an embodiment of the present invention. DETAILED DESCRIPTION

[0058] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of 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. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0059] In the embodiments of the present application, the term "and / or" is merely a description of the association relationship between associated objects, indicating that three relationships may exist. For example, A and / or B may represent three situations: A exists alone, A and B exist at the same time, and B exists alone.

[0060] The terms "first" and "second" in the embodiments of the present application are only used for descriptive purposes and are not to be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of the present application, the terms "include" and "have" and any variations thereof are intended to cover non-exclusive inclusions. For example, a system, product or device comprising a series of components or units is not limited to the listed components or units, but may optionally also include components or units that are not listed, or may optionally also include other components or units that are inherent to these products or devices. In the description of the present application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0061] References herein to "embodiments" mean that a particular feature, structure, or characteristic described in connection with the embodiments may be included in at least one embodiment of the present application. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor does it constitute an independent or alternative embodiment that is mutually exclusive of other embodiments. It is understood, both explicitly and implicitly, by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0062] Existing technologies primarily build environmental maps by fusing LiDAR with inertial navigation and GPS data. However, current mainstream mapping frameworks rely heavily on the assumption that a large number of planes exist in the environment, resulting in reduced mapping performance in some scenarios. For example, trees and utility poles, common in autonomous driving environments, can reduce the number of effective planar features and the number of effective constraints on posture, leading to map drift and even mapping failure.

[0063] To further improve the accuracy and stability of mapping, methods have emerged that perform uncertainty analysis on planes in the environment to improve the accuracy of plane estimation. However, these methods cannot completely break away from their reliance on planar structured environments.

[0064] Some methods have also emerged to identify environmental degradation, such as analyzing the information matrix to determine the scene's positioning constraints and thereby correcting estimates of certain degrees of freedom. However, this approach relies on the sufficiency and accuracy of constraints on non-degenerate dimensions and fails if the number of relevant features in the scene is insufficient.

[0065] To address these issues, the present invention proposes a map construction method and system based on continuous time and surface features. By leveraging surface features in the environment and incorporating the concept of continuous time pose, this method improves the accuracy and stability of map construction, reducing reliance on planar structured environments and inertial measurement units. The present invention's method will be described below through one or more embodiments.

[0066] The embodiment of the present invention provides a map construction method based on continuous time and surface features, such as Figure 1 Shown, including:

[0067] S1. Determine the world pose of the lidar at the start and end times of the point cloud data of the current frame, determine the continuous-time pose based on the world pose of the lidar at the start and end times, and convert the point cloud data of the current frame from the lidar coordinate system to the world coordinate system using the continuous-time pose; wherein the point cloud data is a collection of environmental point clouds collected by the lidar within one scanning cycle;

[0068] Specifically, each frame of point cloud data records three-dimensional coordinate information (in the LiDAR coordinate system) and the acquisition time stamp. Pose refers to the position (translation vector) and posture (rotation matrix / quaternion) of the LiDAR in the world coordinate system, which is used to describe the three-dimensional coordinates and orientation of the LiDAR in the global space. The world coordinate system is a fixed global reference coordinate system (such as the UTM coordinate system or a custom global coordinate system). The environmental point cloud refers to a single three-dimensional point in the current frame of point cloud data, representing the surface points of environmental objects scanned by the LiDAR (such as the surface points of trees and utility poles).

[0069] Because collecting a frame of point cloud takes a certain amount of time (e.g., 0.1 seconds), the LiDAR may move during this period (e.g., the vehicle moves or rotates while driving), and the actual position of the point cloud collected at different times in the world coordinate system will change over time. Directly using a single pose (e.g., the pose at the end) to transform an entire frame of point cloud will cause the point cloud to be "stretched" or "distorted" (motion distortion). Therefore, a single pose cannot accurately describe the coordinate transformation relationship of an entire frame of point cloud.

[0070] The continuous time pose is obtained by the pose at the start of the frame (T a ) and the pose at the end of the frame (T e ) constructs a time-continuous pose model, which can calculate the instantaneous pose at any time within the frame by interpolation, thereby eliminating motion distortion.

[0071] In step S1, after obtaining a frame of point cloud data collected by the lidar, the original point cloud data is downsampled through voxel filtering, and the environmental point cloud is assigned to the corresponding voxels through a three-dimensional spatial hash table, and only the centroid point in each voxel is retained as the representative point.

[0072] When a lidar captures a point cloud frame (a collection of environmental points within a scanning cycle), its pose changes over time. Using only a single pose to transform the entire point cloud frame will result in motion distortion (such as point cloud stretching or offset). Continuous-time pose models pose continuity in the time dimension (such as a smooth transition from the starting pose to the ending pose), allowing the instantaneous pose at any time within the frame to be calculated. This allows each environmental point cloud to be accurately transformed from the lidar coordinate system to the world coordinate system.

[0073] Obtain the world pose of the lidar at the start and end moments of the point cloud data, and construct a continuous-time pose based on the world pose of the lidar at the start and end moments; for each environmental point cloud in a frame of point cloud data, determine the instantaneous pose of the lidar at the acquisition moment through linear interpolation and smooth spherical linear interpolation methods according to the relative position of the environmental point cloud at the acquisition moment within the scanning cycle, and convert the corresponding environmental point cloud from the lidar coordinate system to the world coordinate system based on the instantaneous pose of the lidar.

[0074] Unlike the traditional simplified model of "one pose corresponds to one frame of point cloud," continuous-time pose more realistically reflects the motion state of the LiDAR by modeling the continuous change of pose over time. This effectively eliminates motion distortion (such as point cloud distortion caused by high-speed vehicle travel), providing a more accurate point cloud data foundation for subsequent surface fitting. This modeling approach also reduces reliance on the inertial measurement unit (IMU), lowering hardware cost and system complexity (the technical briefing states, "It eliminates the reliance of most mapping frameworks on IMUs").

[0075] For example, assume that a lidar collects a frame of point cloud data from time T0 to time T1 within a scanning period, and the world poses of the lidar at times T0 and T1 are known as P0 and P1 respectively. For a certain environmental point cloud collected at time T i (T0 < T i <T1), first calculate its relative position s=(T i -T0) / (T1 - T0) within the scanning period at the collection time, then calculate the instantaneous pose of the translational part through linear interpolation and the instantaneous pose of the rotational part through smooth spherical linear interpolation, so as to obtain the instantaneous pose of the lidar at time T i . Finally, use this instantaneous pose to transform this environmental point cloud from the lidar coordinate system to the world coordinate system.

[0076] S2. Register the point cloud data of the current frame to the constructed local point cloud map, query the set of neighboring points of the environmental point cloud in the point cloud data, construct a local tangent space coordinate system based on the set of neighboring points, transform the coordinates of the set of neighboring points to the local tangent space coordinate system and then construct an overdetermined system of equations, and fit to obtain the coefficients of the quadratic polynomial surface;

[0077] Specifically, the registration of point cloud data refers to integrating the current frame of point cloud data processed through step S1 (distortion removal and transformation to the world coordinate system) into the constructed local point cloud map, realizing the spatial association of newly collected point cloud data and historical point cloud data, and providing a data basis for subsequent neighboring point queries. Among them, the local point cloud map is a dynamic map storing historical point cloud data within a certain range, adopting a hash voxel structure, that is, allocating environmental point clouds to corresponding voxels through a three-dimensional spatial hash table, and each voxel stores a small number of representative points (such as centroid points), taking into account both storage efficiency and query speed. Through registration, the current frame of point cloud data is associated with environmental historical information, enabling each environmental point cloud to find neighboring points reflecting the surrounding geometric structure in the local map, providing a reference basis for surface fitting. The set of neighboring points is a set of points with similar spatial positions to the current environmental point cloud in the local point cloud map, used to reflect the geometric characteristics of the local area where the environmental point cloud is located.

[0078] The local tangent space coordinate system is a local reference coordinate system constructed based on the geometric distribution of the set of neighboring points, and its axis directions are determined by the spatial distribution characteristics of the neighboring points (such as normal vectors, main extension directions), used to simplify the mathematical expression of the surface.

[0079] An overdetermined system of equations is a system of equations containing more unknowns than the number of unknowns. In this embodiment, it is constructed based on the coordinates of neighboring points in the local tangent space and is used to solve the coefficients of the quadratic polynomial surface.

[0080] In this step, the original point cloud data is converted into quantifiable surface features (quadratic polynomial coefficients) through the process of "point cloud registration - nearest neighbor query - local coordinate system construction - surface fitting", breaking through the limitations of traditional methods that rely on planar features. These surface features can provide stable constraints in non-planar scenes such as trees and utility poles, laying a key foundation for improving the accuracy and environmental adaptability of map construction.

[0081] S3, construct the residual of the environment point cloud to the quadratic polynomial surface, and determine the continuous time pose with the minimum residual;

[0082] Specifically, the residual essentially reflects the degree of spatial matching between the environment point cloud and the local surface. The smaller the residual, the closer the position of the environment point cloud matches the surface features (i.e., the real environment geometry) in the current pose. The larger the residual, the deviation in the pose needs to be corrected.

[0083] Step S3, through the "residual construction → optimization solution" process, converts the surface features extracted in step S2 into quantitative constraints on the pose. The residual describes the deviation between the point and the surface in the current pose, and the process of minimizing the residual is essentially to find the pose that best matches the surrounding surface features. This process fully utilizes the geometric information of the surface features, improving the accuracy and stability of mapping in non-planar scenes. At the same time, through continuous-time pose modeling, it effectively integrates motion information in the temporal dimension.

[0084] S4. Repeat steps S1 to S3 until the preset number of iterations is reached or the change in the continuous-time pose is less than the set threshold, and the optimized continuous-time pose is obtained. According to the optimized continuous-time pose, the point cloud data of the current frame is converted from the lidar coordinate system to the world coordinate system to obtain the final global point cloud map.

[0085] Specifically, the change of continuous time posture is less than the set threshold: it refers to the continuous time posture obtained by two adjacent iterations (the posture at the starting time T a and the end time pose T e ) (such as rotation angle difference, translation distance difference) is less than a preset value (such as rotation difference <0.1°, translation difference <0.01m), indicating that the posture has stabilized and no further iteration is needed.

[0086] When executing S1-S3 for the first time, the initial pose (e.g., obtained by odometry or a priori estimation) may contain errors, resulting in inaccurate neighbor point query in step S2 (e.g., mismatching non-homologous points) and insufficient residual optimization in step S3. By repeating the iterations, each time re-performing the coordinate transformation, surface fitting, and residual calculation based on the pose after the previous round of optimization, the errors can be gradually corrected:

[0087] For example, if there is a translation deviation in the initial pose, causing the environment point cloud to deviate from the actual position after being converted to the world coordinate system, the neighboring points queried in step S2 may come from the surfaces of other objects, and the fitted surface deviates greatly from the actual environment; after one iterative optimization, the pose deviation is reduced, the neighboring points are closer to the actual homologous points, the surface fitting is more accurate, and the residual calculation is more reliable, thereby promoting further optimization of the pose.

[0088] The preset number of iterations and the pose change threshold together constitute the convergence condition, ensuring a balance between "accuracy" and "efficiency". When the pose change is less than the threshold, continued iteration will have limited improvement on accuracy and can be terminated early. If convergence is not achieved after reaching the maximum number of iterations, the current optimal pose is used as the result to avoid algorithm stagnation.

[0089] Furthermore, based on the optimized continuous-time pose, the instantaneous pose of each environmental point cloud within the frame (the lidar world pose at the moment of acquisition) is recalculated, and the environmental point cloud is accurately converted from the lidar coordinate system to the world coordinate system (same conversion logic as S1, but using the optimized pose). The global point cloud map is constructed through frame-by-frame optimization and frame-by-frame fusion. Each frame of point cloud data is optimized in step S4 and converted to the world coordinate system. It is then spliced ​​with the existing global map data (such as the historical frame point cloud) to form a three-dimensional map covering the entire environment.

[0090] For example, when an autonomous vehicle is driving, each time the lidar collects a frame of point cloud, the pose is optimized through the S1-S4 process and converted to the world coordinate system, and then it is spliced ​​with the map constructed by the previous frame, gradually expanding the map range, and finally forming a global map that includes environmental features such as roads, trees, and buildings.

[0091] Because the pose optimization of each frame of the point cloud is based on surface feature constraints (step S3), the global map can still maintain high accuracy in non-planar scenes (such as forested areas and areas with dense telephone poles), avoiding the map drift problem of traditional plane-dependent methods in such scenes.

[0092] Step S4 uses an iterative optimization mechanism to gradually correct the initial pose to a high-precision continuous-time pose, and based on this pose, accurately fuses the current frame point cloud data with the global map. Its core value lies in eliminating pose errors through multiple rounds of surface feature constraints, ensuring that the resulting global point cloud map maintains accuracy and stability in complex environments (especially non-planar scenes), providing a reliable environmental perception foundation for applications such as autonomous driving and robotic navigation.

[0093] Based on the above embodiments, as a preferred implementation, Figure 2 As shown in , the step S1 specifically includes:

[0094] S11, after obtaining a frame of point cloud data collected by the laser radar, downsample the original point cloud through voxel filtering, and assign the environment point cloud to the corresponding voxel through a three-dimensional spatial hash table, retaining only the centroid point in each voxel as the representative point;

[0095] Specifically, voxel filtering is a method for simplifying point clouds through spatial gridding: It divides the three-dimensional space into multiple uniform cubes (voxels), processes the raw point cloud within each voxel, retains only representative points, and reduces the number of point clouds. The raw point cloud data collected by LiDAR is dense (e.g., millions of points per second), and direct processing would be computationally prohibitive. Downsampling can reduce the data volume while preserving the geometric characteristics of the environment, improving the computational efficiency of subsequent steps (such as neighbor point querying and surface fitting).

[0096] A three-dimensional spatial hash table is an efficient data structure that maps three-dimensional coordinates to voxel indices. It converts the three-dimensional coordinates of the environment point cloud into the key value of the corresponding voxel through a hash function, thereby realizing the rapid allocation of the environment point cloud to voxels.

[0097] A voxel is a cubic unit of preset size in three-dimensional space (such as 0.1m×0.1m×0.1m). Each voxel serves as the basic storage unit of the environmental point cloud, ensuring that spatially adjacent points are classified into the same or adjacent voxels.

[0098] The centroid is the average coordinate value (i.e., the geometric center) of all the original point clouds within a voxel. Retaining only the centroid for each voxel streamlines the data while still representing the overall spatial position of the point cloud within that voxel, avoiding redundant information interference caused by dense point clouds.

[0099] Suppose a LiDAR captures a raw environmental point cloud of a tree's surface, where a certain voxel contains 100 dense points (all belonging to the same local area of ​​the tree). Voxel filtering is used to calculate the centroid (average x, y, and z coordinates) of these 100 points, retaining only this centroid as the representative point. This process condenses the 100 points into a single point while preserving the geometric location characteristics of the area. Compared to random downsampling, voxel filtering can more stably preserve the spatial distribution characteristics of the original point cloud by retaining the centroid point, avoiding the loss of key geometric information (such as changes in surface curvature).

[0100] S12. Obtain the world pose of the laser radar at the start and end moments of the point cloud data, and construct a continuous-time pose based on the world pose of the laser radar at the start and end moments; for each environmental point cloud in a frame of point cloud data, determine the instantaneous pose of the laser radar at the acquisition moment by linear interpolation and smooth spherical linear interpolation methods according to the relative position of the environmental point cloud at the acquisition moment within the scanning cycle, and convert the corresponding environmental point cloud from the laser radar coordinate system to the world coordinate system based on the instantaneous pose of the laser radar.

[0101] Among them, each environmental point cloud in a frame of point cloud data refers to the collection of all environmental point clouds collected by the lidar in one scanning cycle. Each "environmental point cloud" is a single three-dimensional point (such as a reflection point on the surface of a tree or a telephone pole) and contains the timestamp information of the collection time.

[0102] The relative position of the environmental point cloud collection time within the scanning cycle refers to the time position ratio of the specific moment when a single environmental point cloud is collected relative to the start and end times of the scanning cycle during the entire scanning cycle of the lidar collecting a frame of point cloud data.

[0103] For example, assuming the LiDAR scan cycle for collecting a frame of point cloud starts at 1 second and ends at 2 seconds (the entire cycle lasts 1 second), if an environmental point cloud is collected at 1.5 seconds, then this acquisition moment is in the middle of the entire scan cycle, and its relative position is the middle position; if it is collected at 1.2 seconds, it is approximately 20% of the way between the start and end of the scan cycle.

[0104] This relative position is used to describe the proportion of time that the environmental point cloud is collected during the entire scanning cycle. Based on the known lidar poses at the start and end of the scanning cycle, the instantaneous pose of the lidar at the collection moment can be interpolated and calculated, thereby achieving precise coordinate transformation of the point cloud.

[0105] The time ratio is used to quantify the temporal position of a single point in the acquisition process of a frame of point cloud. It is a bridge connecting the "start and end time pose" and the "instantaneous pose at any time in the frame", providing key parameters for continuous time pose modeling and ensuring the accuracy of point cloud coordinate conversion.

[0106] For the position of the lidar (translation vector), linear interpolation is performed through the relative position to obtain τ i The instantaneous translational pose at each moment in time is interpolated linearly over time, which conforms to the intuitive laws of translational motion. Spherical linear interpolation is used for the LiDAR's pose (rotation matrix) to avoid the uneven rotational angular velocity caused by linear interpolation. Spherical linear interpolation ensures a constant angular velocity during rotation, which is more consistent with actual physical motion.

[0107] Based on the above embodiment, as a preferred implementation, in step S2, registering the point cloud data of the current frame to the constructed local point cloud map and querying the neighboring point set of the environment point cloud in the point cloud data specifically includes:

[0108] Registering the point cloud data of the current frame to the constructed local point cloud map. If the point cloud data of the current frame is the first frame of point cloud data, it is directly registered to the local point cloud map. Otherwise, aligning the point cloud data of the current frame with the historical point cloud data in the local point cloud map to obtain the precise pose of the point cloud data of the current frame in the world coordinate system, and then registering the point cloud data of the current frame to the local point cloud map; wherein the local point cloud map is a point cloud map with a hash voxel structure;

[0109] Specifically, the above steps are to integrate the current frame point cloud data that has been preprocessed (such as coordinate conversion and dedistortion) into the constructed local point cloud map, so that the newly collected point cloud can be spatially associated with the historical point cloud already in the map, providing a data basis for subsequent steps such as querying neighboring points and fitting surfaces.

[0110] Furthermore, if the current data being processed is the first frame of point cloud data collected by the lidar, since there is no historical point cloud data at this time, no matching or calibration is required. This frame of point cloud data can be directly added to the local point cloud map as the initial data of the map.

[0111] Furthermore, if the current frame is not the first frame, a "registration" operation is required first. This compares and matches the point cloud data of the current frame with the historical point cloud data stored in the local point cloud map. The precise position and posture (i.e., precise pose) of the current frame's point cloud in the world coordinate system are calculated. After determining the precise pose, the current frame's point cloud data is added to the local point cloud map to ensure that the newly added point cloud is accurately aligned in space with the existing data in the map.

[0112] In this implementation, the three-dimensional space is divided into many small cubes (voxels). Each voxel is assigned a unique identifier (key) using a hashing algorithm. Point cloud data is then assigned to the corresponding voxel based on its spatial coordinates. This structure allows for efficient storage and management of point clouds, facilitating the subsequent rapid query of neighboring points around a given point.

[0113] For the environmental point cloud in each frame of point cloud data, the spatial index of the hash voxel and the eight-neighborhood search method are used to count the neighboring environmental point clouds within the eight voxels around the environmental point cloud to form a neighbor point set.

[0114] Specifically, in this embodiment, the three-dimensional space is divided into a large number of regular small cubes (voxels), each voxel has a fixed size (such as 0.5 meters × 0.5 meters × 0.5 meters); each voxel is assigned a unique "key value" (similar to an address number) through a hash algorithm, and the environmental point cloud is assigned to the corresponding voxel according to its own three-dimensional coordinates, retaining only a small number of representative points (such as the center of mass point); "spatial indexing" refers to quickly calculating the key value of the voxel through the coordinates of the environmental point cloud, and then locating the storage location of the voxel in the map, without traversing the entire map, greatly improving query efficiency.

[0115] Furthermore, this embodiment limits the neighbor point search range through "eight-neighborhood search". Each voxel has 8 adjacent voxels in three-dimensional space (similar to the 8 blocks around the center block in a Rubik's Cube, such as the voxels in front, behind, left, right, above, below and four diagonal directions of voxel A), which are respectively located in the combined directions along the positive and negative directions of the x-axis, the positive and negative directions of the y-axis, and the positive and negative directions of the z-axis; the search range includes not only the voxel where the current environment point cloud is located, but also these 8 adjacent voxels, ensuring that all points within a certain spatial range around the point are covered; this range setting balances "search accuracy" and "computational amount": if the range is too small, key neighbor points may be missed, resulting in surface fitting distortion; if the range is too large, irrelevant points (such as points on the surface of other objects) will be introduced, increasing the computational burden.

[0116] The neighbor point set is a collection of points with similar spatial locations around the current environment point cloud. Its core functions are:

[0117] 1. Reflect the geometric characteristics of the local area where the environmental point cloud is located (such as the curvature and orientation of the surface);

[0118] 2. Provide the data foundation for the subsequent "construction of a local tangent space coordinate system" and "fitting of a quadratic polynomial surface." For example, the set of neighboring points of a point on a tree's surface all comes from the tree's local surface, and these points can be used to fit the tree's local surface.

[0119] Based on the above embodiment, as a preferred implementation, in step S2, a local tangent space coordinate system is constructed based on the neighboring point set, the coordinates of the neighboring point set are converted to the local tangent space coordinate system, an overdetermined set of equations is constructed, and a surface is obtained by fitting, which specifically includes:

[0120] Calculate the centroid and covariance matrix of the neighboring point set, take the eigenvector corresponding to the minimum eigenvalue as the Z axis; select the eigenvector perpendicular to the Z axis and with the largest eigenvalue as the X axis, and obtain the Y axis by the cross product of the Z axis and the X axis. Make the three axes orthogonal to each other and normalize them to form a right-handed coordinate system;

[0121] Specifically, the centroid is the geometric center of the set of neighboring points, which is obtained by calculating the average value of the coordinates of all neighboring points. The role of the centroid is to simplify the benchmark for subsequent coordinate system construction and avoid the influence of noise from a single point on the overall geometric characteristics.

[0122] The covariance matrix describes the distribution characteristics of neighboring points in three-dimensional space, reflecting the degree of dispersion and correlation of the points in the x, y, and z directions. For example, the neighboring points on a tree surface are more dispersed along the trunk's extension (e.g., the x-axis). The covariance matrix will exhibit a larger eigenvalue in this direction, while the eigenvalue in the direction perpendicular to the surface (e.g., the z-axis) will be smaller. This provides a quantitative basis for the subsequent determination of the coordinate axes.

[0123] After performing eigenvalue decomposition on the covariance matrix, the three eigenvalues ​​obtained correspond to the "degree of discreteness" of the neighboring point set in the direction of the three eigenvectors - the larger the eigenvalue, the more dispersed the point set is in that direction, which is the main extension direction of the point set; the smaller the eigenvalue, the more concentrated the point set is in that direction, which is close to the normal of the plane or surface.

[0124] Taking the nearest neighbor point to the current environment point cloud in the neighbor point set as the origin, using the orthogonal axes formed by the X-axis, Y-axis, and Z-axis to form a transformation matrix, the neighbor point in the world coordinate system is transformed into the local tangent space coordinate system based on the transformation matrix;

[0125] Specifically, the origin is the nearest neighbor point in the neighbor point set to the current environment point cloud, rather than the centroid or other points of the neighbor point set. The core purpose is to make the local coordinate system closer to the actual spatial position of the current environment point cloud.

[0126] For example, if the current environment point cloud is located at a bulge on the surface of a tree, its nearest neighbor must be a point near the bulge. Using this as the origin ensures that the benchmark of the local coordinate system is highly consistent with the geometric features of the current point, reducing errors in subsequent coordinate transformations.

[0127] According to the coordinates of the neighboring point set in the local tangent space coordinate system, an overdetermined system of equations about the coefficient vectors of the quadratic polynomial surface is constructed, and the overdetermined system of equations is solved by the least squares method to obtain the local quadratic polynomial surface coefficients.

[0128] In the local tangent space coordinate system, the surface where the neighboring point set is located is simplified to a form with the Z axis as the normal, so it can be described by a quadratic polynomial surface equation as:

[0129] f(x,y)=[a][x 2 ,y 2 ,xy,x,y]

[0130] [a]=[a1,a2,a3,a4,a5]

[0131] This equation can effectively fit common surface features in the natural environment (such as the arc shape of the tree surface, the cylindrical side of the telephone pole, etc.), and has stronger scene adaptability than the plane equation.

[0132] In the above formula, f(x,y) is the equation of the quadratic polynomial surface fitted in the local tangent space, which is used to describe the surface shape formed by the set of neighboring points. Through this function, the corresponding z coordinate can be calculated according to the x and y coordinates in the local tangent space; x, y, and z represent the x-axis, y-axis, and z-axis coordinates of the neighboring points in the local tangent space coordinate system respectively; [a] represents the coefficient vector of the quadratic polynomial surface, which contains the various coefficients in the quadratic polynomial surface equation and is used to determine the specific shape of the surface; a1, a2, a3, a4, and a5 represent the x in the equation of the quadratic polynomial surface respectively. 2 、y 2 The coefficients of the xy, x, and y terms determine the curvature and tilt direction of the surface. These coefficients are obtained by solving the overdetermined system of equations using the least squares method.

[0133] The coordinates of each point in the neighboring point set in the local tangent space coordinate system satisfy the above quadratic polynomial equation. Therefore, the overdetermined equation group of the coefficient vector [a] is:

[0134]

[0135] In the above formula, (x1,y1,z1),(x2,y2,z2),...,(x n ,y n ,z n ) represents the coordinates of each neighbor point in the neighbor point set in the local tangent space coordinate system.

[0136] Through the design of the above-mentioned overdetermined equations, even if some points have deviations due to noise, the errors can still be offset through overall fitting to ensure the robustness of the surface coefficients.

[0137] The essence of solving the overdetermined system of equations by the least squares method is to find a set of coefficients [a1, a2, a3, a4, a5] that minimizes the sum of squares of the residuals from all neighboring points to the fitting surface.

[0138] For example, the neighboring points on the surface of a tree may have a small number of "outliers" due to the deviation of the lidar scanning angle. The least squares method will weaken the influence of these points and fit a surface that is closer to the distribution of the majority of points, which is more consistent with the geometric characteristics of the real environment.

[0139] Based on the above embodiment, as a preferred implementation, step S3 specifically includes:

[0140] For the environment point cloud in the current frame point cloud data:

[0141]

[0142] Build the residual from the environment point cloud to a quadratic polynomial surface:

[0143]

[0144] In the above formula, e i [X] is the residual of the i-th environment point cloud; represents the three-dimensional coordinates of the i-th environmental point cloud in the laser radar coordinate system in the current frame point cloud data; X = [T a ,T e ] represents a frame of point cloud data at the starting time T a and the end time T e The position of the lidar in the world coordinate system at this time; It is the coordinate of the environment point cloud after being converted to the local tangent space coordinate system; is the predicted value of the local quadratic polynomial surface in the tangent space;

[0145] Based on X, the current frame point cloud data is converted from the lidar coordinate system to the world coordinate system, and then to the local tangent space coordinate system:

[0146]

[0147] In the above formula, is the laser radar coordinate system at τ i The pose in the world coordinate system at the moment, τ i is the collection time of the i-th environment point cloud; Indicates the i-th environment point cloud in the current frame point cloud data, after The three-dimensional coordinates in the local tangent space coordinate system after transformation; is the transformation matrix of the local tangent space coordinate system; is the coordinate of the i-th environment point cloud in the current frame point cloud data in the world coordinate system; is the center point of the neighboring point set; For the rotating part, is the translation part;

[0148] Determine the optimization objective function of the current frame point cloud data:

[0149]

[0150] In the above formula, I n is the number of valid points involved in residual calculation; β is the loss function, which aims to "weaken the influence of outliers". When the residual is too large (such as point cloud noise or mismatching), β slows down its contribution to the total error, preventing outliers from dominating the optimization and making the result more robust. is the square of the residual of the i-th environment point cloud;

[0151] Determine the continuous time pose X with the minimum residual a ,T e ].

[0152] Based on the above embodiment, as a preferred implementation, in step S4, after converting the point cloud data of the current frame from the lidar coordinate system to the world coordinate system according to the optimized continuous-time pose, the step further includes:

[0153] Update the local point cloud map, register the point cloud data to the local point cloud map, traverse the local point cloud map, determine the distance between the map voxel and the corresponding lidar pose, and delete the corresponding map voxel if the determined distance exceeds a preset distance threshold.

[0154] Specifically, after iteratively solving the transformation pose of the current frame point cloud data to the world coordinate system, the local point cloud map needs to be updated. In order to prevent the map result from being too large and taking up too many computer resources, map cropping is required. Specifically, the maximum range parameters of the map are set. When a new point cloud is registered to the map, the map structure is traversed to calculate the distance from the voxel to the current lidar pose. Once the distance exceeds the set parameters, the voxel is deleted. In general, through this strategy, the local map can be maintained within a certain range from the current pose, thereby preventing the map from taking up too many resources.

[0155] This embodiment also provides a map construction system based on continuous time and surface features, based on the map construction method based on continuous time and surface features in the above format example, such as Figure 3 As shown in , the system includes:

[0156] The point cloud preprocessing module 310 determines the world pose of the lidar at the start and end times of the point cloud data of the current frame, determines a continuous-time pose based on the world pose of the lidar at the start and end times, and converts the point cloud data of the current frame from the lidar coordinate system to the world coordinate system using the continuous-time pose; wherein the point cloud data is a collection of environmental point clouds collected by the lidar during a scanning cycle;

[0157] The surface fitting module 320 registers the point cloud data of the current frame to the constructed local point cloud map, queries the neighboring point set of the environment point cloud in the point cloud data, constructs a local tangent space coordinate system based on the neighboring point set, transforms the coordinates of the neighboring point set to the local tangent space coordinate system, constructs an overdetermined system of equations, and fits the quadratic polynomial surface coefficients.

[0158] The residual optimization module 330 constructs the residual of the environment point cloud to the quadratic polynomial surface and determines the continuous time pose with the minimum residual;

[0159] The map update module 340 repeats the steps from the point cloud preprocessing module to the residual optimization module until the preset number of iterations is reached or the change in the continuous-time pose is less than the set threshold, and obtains the optimized continuous-time pose. According to the optimized continuous-time pose, the point cloud data of the current frame is converted from the lidar coordinate system to the world coordinate system to obtain the final global point cloud map.

[0160] Based on the same concept, the embodiment of the present invention also provides a schematic diagram of an entity structure, such as Figure 4 As shown, the server may include: a processor 410, a communication interface 420, a memory 430, and a communication bus 440, wherein the processor 410, the communication interface 420, and the memory 430 communicate with each other via the communication bus 440. The processor 410 may call the logic instructions in the memory 430 to execute the steps of the map construction method based on continuous time and surface features as described in the above embodiments.

[0161] In addition, the logic instructions in the above-mentioned memory 430 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when sold or used as an independent product. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0162] Based on the same concept, an embodiment of the present invention also provides a non-transitory computer-readable storage medium, which stores a computer program. The computer program includes at least one code segment, which can be executed by a main control device to control the main control device to implement the steps of the map construction method based on continuous time and surface features as described in the above embodiments.

[0163] Based on the same technical concept, an embodiment of the present application also provides a computer program, which, when executed by a main control device, is used to implement the above method embodiment.

[0164] The program may be stored in whole or in part on a storage medium packaged with the processor, or may be stored in whole or in part on a memory not packaged with the processor.

[0165] Based on the same technical concept, the embodiment of the present application further provides a processor, which is used to implement the above method embodiment. The above processor can be a chip.

[0166] The various embodiments of the present invention can be combined arbitrarily to achieve different technical effects.

[0167] In the above embodiments, all or part of the embodiments can be implemented by software, hardware, firmware, or any combination thereof. When implemented using software, all or part of the embodiments can be implemented in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the process or function described in this application is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, optical fiber, digital subscriber line) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that includes one or more available media. The available medium can be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a solid-state drive).

[0168] Those skilled in the art will appreciate that all or part of the process steps in the above-described method embodiments can be implemented by a computer program instructing the relevant hardware. The program can be stored in a computer-readable storage medium, and when executed, the program can include the process steps in the above-described method embodiments. The aforementioned storage medium includes various media capable of storing program code, such as ROM or random access memory (RAM), magnetic disks, or optical disks.

[0169] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A map construction method based on continuous time and surface features, characterized in that: include: S1. Determine the world pose of the lidar at the start and end times of the point cloud data of the current frame, determine the continuous-time pose based on the world pose of the lidar at the start and end times, and convert the point cloud data of the current frame from the lidar coordinate system to the world coordinate system using the continuous-time pose; wherein the point cloud data is a collection of environmental point clouds collected by the lidar within one scanning cycle; S2. Registering the point cloud data of the current frame to the constructed local point cloud map, querying the neighboring point set of the environment point cloud in the point cloud data, constructing a local tangent space coordinate system based on the neighboring point set, converting the coordinates of the neighboring point set to the local tangent space coordinate system, constructing an overdetermined system of equations, and fitting to obtain quadratic polynomial surface coefficients; S3, construct the residual of the environment point cloud to the quadratic polynomial surface, and determine the continuous time pose with the minimum residual; S4. Repeat steps S1 to S3 until the preset number of iterations is reached or the change in the continuous-time pose is less than the set threshold, and the optimized continuous-time pose is obtained. According to the optimized continuous-time pose, the point cloud data of the current frame is converted from the lidar coordinate system to the world coordinate system to obtain the final global point cloud map.

2. The map construction method based on continuous time and surface features according to claim 1, characterized in that: The step S1 specifically includes: S11, after obtaining a frame of point cloud data collected by the laser radar, downsample the original point cloud through voxel filtering, and assign the environment point cloud to the corresponding voxel through a three-dimensional spatial hash table, retaining only the centroid point in each voxel as the representative point; S12. Obtain the world pose of the laser radar at the start and end moments of the point cloud data, and construct a continuous-time pose based on the world pose of the laser radar at the start and end moments; for each environmental point cloud in a frame of point cloud data, determine the instantaneous pose of the laser radar at the acquisition moment by linear interpolation and smooth spherical linear interpolation methods according to the relative position of the environmental point cloud at the acquisition moment within the scanning cycle, and convert the corresponding environmental point cloud from the laser radar coordinate system to the world coordinate system based on the instantaneous pose of the laser radar.

3. The map construction method based on continuous time and surface features according to claim 1, characterized in that: In step S2, the point cloud data of the current frame is registered to the constructed local point cloud map, and the neighboring point set of the environment point cloud in the point cloud data is queried, which specifically includes: Registering the point cloud data of the current frame to the constructed local point cloud map. If the point cloud data of the current frame is the first frame of point cloud data, it is directly registered to the local point cloud map. Otherwise, aligning the point cloud data of the current frame with the historical point cloud data in the local point cloud map to obtain the precise pose of the point cloud data of the current frame in the world coordinate system, and then registering the point cloud data of the current frame to the local point cloud map; wherein the local point cloud map is a point cloud map with a hash voxel structure; For the environmental point cloud in each frame of point cloud data, the spatial index of the hash voxel and the eight-neighborhood search method are used to count the neighboring environmental point clouds within the eight voxels around the environmental point cloud to form a neighbor point set.

4. The map construction method based on continuous time and surface features according to claim 1, characterized in that: In step S2, a local tangent space coordinate system is constructed based on the neighboring point set, the coordinates of the neighboring point set are converted to the local tangent space coordinate system, an overdetermined set of equations is constructed, and a surface is obtained by fitting, which specifically includes: Calculate the centroid and covariance matrix of the neighboring point set, take the eigenvector corresponding to the minimum eigenvalue as the Z axis; select the eigenvector perpendicular to the Z axis and with the largest eigenvalue as the X axis, and obtain the Y axis by the cross product of the Z axis and the X axis. Make the three axes orthogonal to each other and normalize them to form a right-handed coordinate system; Taking the nearest neighbor point to the current environment point cloud in the neighbor point set as the origin, using the orthogonal axes formed by the X-axis, Y-axis, and Z-axis to form a transformation matrix, the neighbor point in the world coordinate system is transformed into the local tangent space coordinate system based on the transformation matrix; According to the coordinates of the neighboring point set in the local tangent space coordinate system, an overdetermined system of equations about the coefficient vectors of the quadratic polynomial surface is constructed, and the overdetermined system of equations is solved by the least squares method to obtain the local quadratic polynomial surface coefficients.

5. The map construction method based on continuous time and surface features according to claim 4, characterized in that: In step S2, the equation of the quadratic polynomial surface is: f(x,y)=[a][x 2 ,y 2 ,xy,x,y] [a]=[a1,a2,a3,a4,a5] In the above formula, f(x,y) is the equation of the quadratic polynomial surface fitted in the local tangent space; x, y, and z represent the x-axis, y-axis, and z-axis coordinates of the neighboring points in the local tangent space coordinate system, respectively; [a] represents the coefficient vector of the quadratic polynomial surface, and a1, a2, a3, a4, and a5 represent the x-axis in the equation of the quadratic polynomial surface. 2 、y 2 , xy, coefficients of x, y terms; The overdetermined system of equations for the coefficient vector [a] is: In the above formula, (x1,y1,z1),(x2,y2,z2),...,(x n ,y n ,z n ) represents the coordinates of each neighbor point in the neighbor point set in the local tangent space coordinate system.

6. The map construction method based on continuous time and surface features according to claim 5, characterized in that: The step S3 specifically includes: For the environment point cloud in the current frame point cloud data: Build the residual from the environment point cloud to a quadratic polynomial surface: In the above formula, e i [X] is the residual of the i-th environment point cloud; represents the three-dimensional coordinates of the i-th environmental point cloud in the laser radar coordinate system in the current frame point cloud data; X = [T a ,T e ] represents a frame of point cloud data at the starting time T a and the end time T e The position of the lidar in the world coordinate system at this time; It is the coordinate of the environment point cloud after being converted to the local tangent space coordinate system; is the predicted value of the local quadratic polynomial surface in the tangent space; Based on X, the current frame point cloud data is converted from the lidar coordinate system to the world coordinate system, and then to the local tangent space coordinate system: In the above formula, is the laser radar coordinate system at τ i The pose in the world coordinate system at the moment, τ i is the collection time of the i-th environment point cloud; Indicates the i-th environment point cloud in the current frame point cloud data, after The three-dimensional coordinates in the local tangent space coordinate system after transformation; is the transformation matrix of the local tangent space coordinate system; is the coordinate of the i-th environment point cloud in the current frame point cloud data in the world coordinate system; is the center point of the neighboring point set; For the rotating part, is the translation part; Determine the optimization objective function of the current frame point cloud data: In the above formula, I n is the number of valid points involved in residual calculation; β is the loss function; is the square of the residual of the i-th environment point cloud; Determine the continuous time pose X with the minimum residual a ,T e ].

7. The map construction method based on continuous time and surface features according to claim 1, characterized in that: In step S4, after converting the point cloud data of the current frame from the lidar coordinate system to the world coordinate system according to the optimized continuous-time pose, the following steps are further included: Update the local point cloud map, register the point cloud data to the local point cloud map, traverse the local point cloud map, determine the distance between the map voxel and the corresponding lidar pose, and delete the corresponding map voxel if the determined distance exceeds a preset distance threshold.

8. A map construction system based on continuous time and surface features, characterized in that: include: a point cloud preprocessing module for determining the world pose of the lidar at the start and end times of the point cloud data of the current frame, determining a continuous-time pose based on the world pose of the lidar at the start and end times, and converting the point cloud data of the current frame from the lidar coordinate system to the world coordinate system using the continuous-time pose; wherein the point cloud data is a collection of environmental point clouds collected by the lidar within one scanning cycle; a surface fitting module that registers the point cloud data of the current frame to a constructed local point cloud map, queries the neighboring point set of the environment point cloud in the point cloud data, constructs a local tangent space coordinate system based on the neighboring point set, transforms the coordinates of the neighboring point set to the local tangent space coordinate system, constructs an overdetermined system of equations, and fits the quadratic polynomial surface coefficients; The residual optimization module constructs the residual from the environment point cloud to the quadratic polynomial surface and determines the continuous time pose with the minimum residual; The map update module repeats the steps from the point cloud preprocessing module to the residual optimization module until the preset number of iterations is reached or the change in the continuous-time pose is less than the set threshold, and obtains the optimized continuous-time pose. According to the optimized continuous-time pose, the point cloud data of the current frame is converted from the lidar coordinate system to the world coordinate system to obtain the final global point cloud map.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the steps of the map construction method based on continuous time and surface features as claimed in any one of claims 1 to 7 are implemented.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the map construction method based on continuous time and surface features as claimed in any one of claims 1 to 7 are implemented.

Images