Method and device for establishing point cloud map, unmanned vehicle and storage medium

Abstract

The invention discloses a point cloud map establishing method and device, an unmanned vehicle and a storage medium, and the method comprises the steps: obtaining the point cloud data of a target region and the IMU data of a target vehicle, and determining the neighborhood space of each point in the point cloud data; calculating a covariance matrix of the point cloud in the neighborhood space, and decomposing the covariance matrix to obtain a corresponding characteristic value; calculating the weighted curvature of the corresponding point based on the characteristic value and the point cloud depth and geometric residual error of the corresponding point; and screening the point cloud data based on the weighted curvature, and fusing the obtained reference point cloud data with the IMU data to establish a point cloud map. Therefore, mapping can be realized in a complex scene, especially in an environment including dense forests and bushes, and the mapping efficiency is relatively high.

Description

Technical Field

[0001] This application belongs to the technical field of autonomous driving, and particularly relates to a method and device for building a point cloud map, an autonomous vehicle, and a storage medium. Background Art

[0002] Currently, high-precision maps are increasingly widely used in autonomous driving. In some complex scenarios, such as environments including dense forests and bushes, existing mapping methods often cannot meet the requirements.

[0003] The information disclosed in this background art section is only intended to enhance the overall understanding of this application and should not be regarded as an admission or any form of implication that this information constitutes prior art already known to those of ordinary skill in the art. Summary of the Invention

[0004] The purpose of this application is to provide a method for building a point cloud map, which is used to solve the problem that existing mapping methods cannot meet the mapping requirements in complex scenarios including dense forests and bush environments.

[0005] To achieve the above purpose, this application provides a method for building a point cloud map, and the method includes:

[0006] Obtain the point cloud data of the target area and the IMU data of the target vehicle, and determine the neighborhood space of each point in the point cloud data;

[0007] Calculate the covariance matrix of the point cloud in the neighborhood space, and decompose it to obtain the corresponding eigenvalues;

[0008] Based on the eigenvalues, as well as the point cloud depth and geometric residual of the corresponding point, calculate the weighted curvature of the corresponding point;

[0009] Filter the point cloud data based on the weighted curvature, and fuse the obtained reference point cloud data with the IMU data to build a point cloud map.

[0010] In one embodiment, calculating the weighted curvature of the corresponding point based on the eigenvalues, as well as the point cloud depth and geometric residual of the corresponding point, specifically includes:

[0011] Construct a first function sub-item based on the eigenvalues, where the first function sub-item represents the curvature of the corresponding point;

[0012] Construct a second function sub-item based on the point cloud depth and geometric residual of the corresponding point, where the value of the second function sub-item is inversely related to the value of the point cloud depth;

[0013] Based on the first function sub-item and the second function sub-item, calculate the weighted curvature of the corresponding point.

[0014] In one embodiment, the second function sub-item includes a proportional function of the point cloud depth and geometric residual of the corresponding points.

[0015] In one embodiment, obtaining the point cloud data of the target area and the IMU data of the target vehicle, and determining the neighborhood space of each point in the point cloud data specifically includes:

[0016] Based on the elevation information of the point cloud data, dividing the point cloud data into ground point cloud data and non-ground point cloud data;

[0017] Determining the neighborhood space of each point in the non-ground point cloud data;

[0018] Based on the weighted curvature, screening the point cloud data, and fusing the obtained reference point cloud data with the IMU data to establish a point cloud map, specifically including:

[0019] Based on the weighted curvature, screening the non-ground point cloud data to obtain reference point cloud data;

[0020] Merging the reference point cloud data and the ground point cloud data, and fusing the obtained merged point cloud data with the IMU data to establish a point cloud map.

[0021] In one embodiment, fusing the obtained reference point cloud data with the IMU data to establish a point cloud map, specifically including:

[0022] Selecting target IMU data located between point cloud data frames from the IMU data for pose prediction to obtain the current predicted pose of the target area point cloud data;

[0023] Projecting the merged point cloud data into the coordinate system of the current predicted pose, and calculating the geometric residual of each point therein;

[0024] Based on the geometric residual of the merged point cloud data, iteratively updating the merged point cloud data to establish a point cloud map.

[0025] In one embodiment, the method specifically includes:

[0026] Predicting the current covariance prediction matrix based on the target IMU data, and calculating the Jacobian matrix based on the geometric residual of the merged point cloud data;

[0027] Based on the covariance prediction matrix and the Jacobian matrix, determining the current Kalman gain;

[0028] Based on the current Kalman gain and the geometric residual of the merged point cloud data, performing pose update on the merged point cloud data to obtain the updated current predicted pose.

[0029] In one embodiment, the method specifically includes:

[0030] Fit a reference plane and / or a reference line based on the point cloud within the neighborhood space;

[0031] Calculate the distance from the corresponding point to the reference plane and / or the reference line, and use it as the geometric residual.

[0032] The present application also provides a device for establishing a point cloud map, including:

[0033] An acquisition module, configured to acquire the point cloud data and IMU data of the target area, and determine the neighborhood space of each point in the point cloud data;

[0034] A first calculation module, configured to calculate the covariance matrix of the point cloud within the neighborhood space, and decompose it to obtain the corresponding eigenvalues;

[0035] A second calculation module, configured to calculate the weighted curvature of the corresponding point based on the eigenvalue, as well as the point cloud depth and geometric residual of the corresponding point;

[0036] A map establishment module, configured to screen the point cloud data based on the weighted curvature, and fuse the obtained reference point cloud data with the IMU data to establish a point cloud map.

[0037] The present application also provides an autonomous vehicle, including:

[0038] At least one processor; and

[0039] A memory, the memory stores instructions, when the instructions are executed by the at least one processor, the at least one processor is caused to execute the method for establishing a point cloud map as described above.

[0040] The present application also provides a machine-readable storage medium, which stores executable instructions, and when the instructions are executed, the machine is caused to execute the method for establishing a point cloud map as described above.

[0041] Compared with the prior art, according to the method for establishing a point cloud map of the present application, by determining the neighborhood space of each point in the point cloud data within the target area, then calculating the covariance matrix of the point cloud within the neighborhood space respectively to decompose and obtain the corresponding eigenvalues, and further calculating the weighted curvature of the corresponding point based on the eigenvalues, as well as the point cloud depth and geometric residual of the corresponding point; the point cloud data can be screened based on the weighted curvature to obtain reference point cloud data with a clearer structure, and then fused with the IMU data to achieve mapping, improving the mapping success rate in complex environments, especially in scenes including dense forests and bushes, and having a higher mapping efficiency.

[0042] In another aspect, when fusing IMU data and point cloud data, target IMU data located between point cloud frames is selected from the IMU data for pose prediction, and the merged point cloud data is projected onto the coordinate system of the predicted pose, and the geometric residuals of each point therein are calculated, and then the merged point cloud data is iteratively updated to achieve mapping. In this way, the high-frequency characteristics of the IMU data are utilized, short-term motion priors are provided based on the high-frequency prediction of the IMU, and then the long-term drift is corrected by combining with the geometric residuals of the laser point cloud data, realizing the complementarity of the IMU data and the point cloud data and improving the accuracy and robustness of the inference. Description of the Drawings

[0043] Figure 1 FIG. is an application scenario diagram of a method for establishing a point cloud map according to an embodiment of the present application;

[0044] Figure 2 FIG. is a flowchart of a method for establishing a point cloud map according to an embodiment of the present application;

[0045] Figure 3 FIG. is a scenario diagram of point cloud screening in a method for establishing a point cloud map according to an embodiment of the present application;

[0046] Figure 4 FIG. is a schematic flowchart of fusing IMU data and point cloud data in a method for establishing a point cloud map according to an embodiment of the present application;

[0047] Figure 5 FIG. is a schematic diagram of mapping a certain area in a method for establishing a point cloud map according to an embodiment of the present application;

[0048] Figure 6 Module diagram of a device for establishing a point cloud map according to an embodiment of the present application;

[0049] Figure 7 FIG. is a hardware structure diagram of an autonomous vehicle according to an embodiment of the present application. Detailed Embodiments

[0050] The present application will be described in detail below with reference to the various embodiments shown in the drawings. However, these embodiments do not limit the present application, and any structural, method, or functional transformation made by those of ordinary skill in the art based on these embodiments is included in the protection scope of the present application.

[0051] In the description, claims, and above-mentioned drawings of the present application, terms such as "first", "second", "third", "fourth", etc. (if any) are used to distinguish similar objects and do not necessarily describe a specific order or sequence. It should be understood that the data used in this way can be interchanged under appropriate circumstances so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "comprising" and "corresponding to" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units does not necessarily limit to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products, or devices.

[0052] Before introducing the embodiments of the present application, a schematic explanation is given to the basic technologies and some technical terms involved in the embodiments of the present application:

[0053] Autopilot: It refers to the function that can guide and make decisions on the vehicle driving tasks and replace the test driver's control behavior to enable the vehicle to complete safe driving without the test driver performing physical driving operations. Autopilot technology usually includes technologies such as high-precision maps, environmental perception, behavior decision-making, path planning, and motion control.

[0054] Autopilot system: A system that realizes different levels of autopilot functions of the vehicle, such as an assisted driving system (L2), a high-speed autopilot system that requires human supervision (L3), and a highly / fully autonomous driving system (L4 / L5).

[0055] Point cloud data refers to a set of vectors in a three-dimensional coordinate system. Among them, this set is recorded in the form of points, and each point contains three-dimensional coordinates and can carry other information about the attributes of this point, such as color, reflectivity, intensity, etc. Point cloud data is usually obtained by devices such as laser scanners, cameras, and three-dimensional scanners and can be used in applications such as three-dimensional modeling, scene reconstruction, robot navigation, virtual reality, and augmented reality.

[0056] The main characteristics of point cloud data are high-precision, high-resolution, and high-dimensional geometric information, which can intuitively represent information such as the shape, surface, and texture of objects in space. The processing and analysis of point cloud data usually require the use of computer vision and computer graphics technologies, such as point cloud filtering, registration, segmentation, reconstruction, recognition, and classification.

[0057] IMU data (Inertial Measurement Unit data): The data of the inertial measurement unit can be obtained by sensors such as accelerometers, gyroscopes, and magnetometers. IMU data usually includes acceleration and angular velocity data, and is combined with other sensors (such as GNSS, LiDAR) to achieve attitude estimation, motion tracking, and positioning compensation.

[0058] Figure 1 This is an optional schematic diagram of the system architecture involved in the embodiments of the present application. As Figure 1 shown, the lidar collects the point cloud data of the current environment, and the inertial measurement unit collects the IMU data of the target vehicle. The lidar sends the collected point cloud data to the server, and the inertial measurement unit sends the collected IMU data to the server. After receiving the point cloud data and IMU data, the server fuses the two to establish a point cloud map of the current environment. The server can send the established point cloud map to terminal devices such as in-vehicle terminals and user terminals.

[0059] In the above architecture, the lidar can be installed on roadside equipment or on-vehicle radar. The inertial measurement unit can be directly installed on the vehicle body (such as the centroid). The server can be a third-party server, such as the server of an enterprise, which is used to perform point cloud mapping on the target area according to the data collected by the lidar and the inertial measurement unit; the server can also be an in-vehicle device. For example, if the autonomous vehicle already has a map, the in-vehicle device directly performs point cloud mapping according to the data collected by the lidar and the inertial measurement unit.

[0060] Continuing to refer Figure 1 to, in a specific scenario of applying the method for establishing the point cloud map of the present application, it may include a server and terminal devices. Among them, the terminal devices can include in-vehicle terminals and user terminals. The in-vehicle terminal can include an on-board computer or an on-board unit (OBU), etc. The in-vehicle terminal can also be an application (APP) on the terminal, an APP on the smart rearview mirror, an APP on the mobile phone, or a small program, etc., which is not limited herein. The user terminal (UE) can be a wireless terminal device or a wired terminal device. The wireless terminal device can refer to a device with wireless transceiver functions. The user terminal can be a mobile phone, a tablet computer (Pad), a computer with wireless transceiver functions, a virtual reality (VR) user device, an augmented reality (AR) user device, a smart voice interaction device, a smart home appliance, an in-vehicle terminal, an aircraft, etc., which is not limited herein.

[0061] For example, a smartphone with a mobile navigation app installed can be used as a terminal device. The server can send the generated point cloud map to the smartphone. Simultaneously, the autonomous vehicle combines data from other sensors to determine the drivable road surfaces in the current environment. The smartphone navigation app can also display real-time vehicle location information.

[0062] For example, the terminal device is a vehicle-mounted device with an in-vehicle navigation application installed. The server can send the generated point cloud map to the vehicle-mounted device. Simultaneously, the vehicle-mounted device combines detection data from other sensors to determine the current drivable road surface. The navigation application on the vehicle-mounted device can also display real-time vehicle location information.

[0063] It is understood that the above is only an example of a server to illustrate one possible execution entity of the method for establishing a point cloud map of this application. In more embodiments, the establishment of the point cloud map can also be directly executed by a terminal device with sufficient computing power, and this application does not limit this. Moreover, regardless of the type of server / terminal device, the method for establishing a point cloud map provided in the embodiment of this application can be adapted to the autonomous driving system of unmanned vehicles, including L2, L3, L4 and above autonomous driving systems.

[0064] In this application, the establishment of a point cloud map is based on the fusion of point cloud data and IMU data. In different embodiments, the fusion of point cloud data and IMU data can be implemented in different ways. For example, the point cloud data and IMU data are processed independently and then interact only through high-level information such as posture or velocity. For another example, the point cloud data and IMU data are directly jointly optimized, and the pre-integration constraints of the IMU data are embedded in the residual equation of the point cloud data matching. The embodiments shown in this application do not limit the specific fusion method.

[0065] With ginseng Figure 3 In complex environments, including dense forests and bushes, the quality of the point cloud data during fusion has a more significant impact on the success and quality of mapping. This is especially true when point cloud data and IMU data are directly jointly optimized. Point-to-surface residual features can only take into account adjacent partial point clouds. Dense forest and bush point clouds cannot be filtered out, leading to mapping failure or poor quality. Therefore, embodiments of this application aim to filter points in the point cloud based on weighted curvature to obtain point clouds with greater distances and clearer geometric structures, thereby achieving fast and robust odometry inference and improving the success rate and quality of mapping.

[0066] Specific reference Figure 2 , introduces an embodiment of the method for establishing a point cloud map of the present application. In this embodiment, the method includes:

[0067] S11. Obtain the point cloud data of the target area and the IMU data of the target vehicle, and determine the neighborhood space of each point in the point cloud data.

[0068] Cooperation parameter Figure 4 , the point cloud data can be a single-frame point cloud scanned by a lidar at a certain moment, or a set of multi-frame point clouds periodically scanned by a lidar within a set period. The method for establishing the point cloud map provided in each embodiment of the present application can be based on a single-frame point cloud or a set of multi-frame point clouds. The target area can be the coverage range of the point cloud data scanned by the lidar. Correspondingly, if the lidar is installed on an unmanned vehicle, the multi-frame point clouds obtained during the driving of the unmanned vehicle can include point cloud data with a larger range relative to a single-frame point cloud. The present application does not limit the range or scanning duration of the target area.

[0069] The IMU data corresponds to the point cloud data, and the target vehicle synchronously obtains the IMU data when obtaining the point cloud data. Among them, the point cloud data usually has a relatively low frequency, such as 10Hz, and the IMU data usually has a relatively high frequency, such as 100Hz.

[0070] The neighborhood space of each point in the point cloud data can be determined in various ways. For example, a certain range of space can be determined with the corresponding point as the center; for another example, the range of the neighborhood space can be determined by the adjacent points in the point cloud, calculate the N point cloud data adjacent to the corresponding point, and use the range defined by these N point cloud data as the neighborhood space corresponding to this point. The neighborhood spaces corresponding to different points can overlap or not overlap with each other, and the present application does not limit this.

[0071] In one embodiment, the radius search method can be used to determine the neighborhood space. By querying the neighborhood points whose distance from the corresponding point is less than a given radius (such as 300m), the neighborhood point set is determined and used as the neighborhood space corresponding to the corresponding point. In another embodiment, the K-nearest neighbor method can also be used to determine the neighborhood space. By querying the K points closest to the corresponding point, the neighborhood point set is determined and used as the neighborhood space corresponding to the corresponding point.

[0072] In this step, the determination of the neighborhood space of the points in the point cloud is mainly used for subsequent screening of the point clouds of non-woods and bushes. In the actual vehicle driving scenario, the point cloud data may also include information such as building walls, roadside lamp posts, and road surfaces. Among these, the road surface usually has a relatively small elevation compared to other targets. Therefore, in this embodiment, it is further proposed that based on the elevation information of the point cloud data, the point cloud data is divided into ground point cloud data and non-ground point cloud data, and only the neighborhood space of each point in the non-ground point cloud data is determined. By assuming that the point cloud with an elevation lower than the set threshold is the ground point cloud and distinguishing the ground point cloud and the non-ground point cloud in this way, the screening range of the non-woods and bushes point clouds can be reduced, and the mapping speed can be accelerated.

[0073] The set threshold for screening non-ground point clouds can be determined based on the actual terrain environment of the target area and the scope of the target area. When the elevation fluctuation of the terrain environment is large, the set threshold can be appropriately increased. The larger the scope of the target area, the larger the corresponding set threshold can be set. For example, the set thresholds are 0.3m, 0.4m, 0.5m, etc. Or, the body factors of the target vehicle can be directly considered, and the vehicle height minus 0.3m, 0.4m, etc. can be directly used as the set threshold.

[0074] S12. Calculate the covariance matrix of the point clouds in the neighborhood space and decompose it to obtain the corresponding eigenvalues.

[0075] S13. Calculate the weighted curvature of the corresponding points based on the eigenvalues, the point cloud depth of the corresponding points, and the geometric residuals.

[0076] When calculating the covariance matrix of the point clouds in the neighborhood space, the original point cloud data can also be preprocessed, such as removing motion distortion, voxel filtering for downsampling, etc. The covariance matrix of the point cloud data can be calculated using the following formula:

[0077] (1)

[0078] where k is the number of points in the neighborhood space, is the centroid of the points in the neighborhood space, and P i is the i-th point in the neighborhood space.

[0079] Since the covariance matrix is a real symmetric positive semi-definite matrix and is a three-dimensional covariance matrix for point cloud data, formula (1) can be decomposed as:

[0080] (2)

[0081] where Λ = diag(λ1, λ2, λ3), and 0 ≤ λ1 ≤ λ2 ≤ λ3, and λ1, λ2, λ3 are the eigenvalues of the covariance matrix; V is an orthogonal matrix, and its column vectors are the eigenvectors of the covariance matrix, representing the main component directions of the data.

[0082] The eigenvalue represents the variance (uncertainty intensity) in the corresponding eigenvector direction. The larger the eigenvalue, the higher the uncertainty in that direction, and vice versa.

[0083] For example, assume that the covariance matrix of the point cloud data in the neighborhood space corresponding to point A is:

[0084]

[0085] Then the eigenvalues are λ1 = 0.05, λ2 = 0.09, λ3 = 0.21, indicating that the uncertainty in the y direction (the second dimension) is the highest, and the uncertainty in the z direction (height) is the lowest.

[0086] In this embodiment, based on the eigenvalues obtained above, a first functional sub-term representing the curvature of the corresponding point can be constructed, expressed as:

[0087] (3)

[0088] where, in Equation (3), σ i is the curvature of the i-th point in the point cloud data. It can be understood that the first functional sub-term represents the curvature of the corresponding point, rather than limiting the first functional sub-term to represent the true value of the curvature of the corresponding point. In different embodiments, the first functional sub-term can also be to represent the relative magnitude of the curvature of the corresponding point. For example, the first functional sub-term can also be expressed as:

[0089] (4)

[0090] where m is an adjustment parameter set to be greater than or equal to 0.

[0091] In this embodiment, a second functional sub-term can be constructed based on the point cloud depth and geometric residual of the corresponding point, and the value of the second functional sub-term is inversely correlated with the value of the point cloud depth.

[0092] The geometric residual of the corresponding point can be the point-to-plane residual or the point-to-line residual. The point-to-plane residual can be applicable to an environment rich in plane features, such as walls, floors, etc.; the point-to-line residual can be applicable to an environment rich in edges or linear structures, such as door frames, corridors, utility poles, tree trunks, etc. In actual applications, one or a combination of the point-to-plane residual or the point-to-line residual can be selected according to the characteristics of the environment. Among them,

[0093] the point-to-plane residual of any point P i can be expressed as:

[0094] (5)

[0095] where T is the pose change matrix, n is the normal vector of the fitted plane in the local map corresponding to the neighborhood space, and q is an arbitrary reference point on the fitted plane, such as the center point of the point cloud in the neighborhood space.

[0096] the point-to-line residual of any point P i can be expressed as:

[0097] (6)

[0098] Among them, d is the direction vector of the fitted straight line in the local map corresponding to the neighborhood space, and q is an arbitrary reference point on the fitted straight line, such as the center point of the point cloud within the neighborhood space.

[0099] When considering both the point-plane residual and the point-line residual simultaneously, the geometric residual can be expressed as:

[0100] (7)

[0101] Among them, α is the weight coefficient corresponding to the point-plane residual, and β is the weight coefficient corresponding to the point-line residual.

[0102] In the second function sub-item, a proportional function of the point cloud depth and the geometric residual of the corresponding point can be set. In this embodiment, the second function sub-item can be expressed as:

[0103] (8)

[0104] Among them, 0 < Q < 1, fd is the geometric residual of the corresponding point, and depth is the point cloud depth of the corresponding point.

[0105] In an actual application scenario, the value of the geometric residual fd of the corresponding point is usually relatively small compared to the point cloud depth depth of the corresponding point. For example, fd = 4 mm and depth = 200 m. Using this proportional function can obtain a second function sub-item value with a larger variation range, providing a more sensitive curvature reference when setting the screening threshold of the weighted curvature, which is beneficial to screening out point cloud data with clearer structures. It can be understood that in some embodiments, the second function sub-item can also be constructed as:

[0106] (9)

[0107] Similarly, it can be understood that in order to construct a second function sub-item that is inversely related to the value of the point cloud depth, in addition to including the proportional function of the point cloud depth and the geometric residual of the corresponding point, the second function sub-item can also adopt various other reasonable forms, such as the inverse log function, etc. For different forms of the second function sub-item, the screening threshold of the weighted curvature can also be adjusted accordingly, and this application does not limit this either.

[0108] In this embodiment, based on the above first function sub-item and second function sub-item, the weighted curvature of the corresponding point can be calculated. Taking the first function sub-item adopting Equation (3) and the second function sub-item adopting Equation (8) as an example, the weighted curvature of the corresponding point can be expressed as:

[0109] (10)

[0110] For another example, if the first function sub-item adopts Equation (4) and the second function sub-item adopts Equation (8), the weighted curvature of the corresponding point can be expressed as:

[0111] (11)

[0112] S14. Filter the point cloud data based on weighted curvature, and fuse the obtained reference point cloud data with the IMU data to establish a point cloud map.

[0113] In this embodiment, the non-ground point cloud data can be filtered based on weighted curvature to obtain reference point cloud data, and then the reference point cloud data and the ground point cloud data are merged, and the obtained merged point cloud data is fused with the IMU data to establish a point cloud map.

[0114] Exemplarily, taking Equation (10) as an example, it is assumed that the non-ground point cloud data includes points A, B, and C. Among them,

[0115] Point A: fd = 5mm, depth = 10m, λ1 = 0.05, λ2 = 0.09, λ3 = 0.21;

[0116] Point B: fd = 4mm, depth = 12m, λ1 = 0.04, λ2 = 0.1, λ3 = 0.18;

[0117] Point C: fd = 2mm, depth = 150m, λ1 = 0.01, λ2 = 0.09, λ3 = 0.21.

[0118] Calculate the weighted curvatures of points A, B, and C to be 0.00333, 0.00298, and 0.00044 respectively. It can be seen that there is an order-of-magnitude difference in the weighted curvature at point C with a larger point cloud depth compared to points A and B with smaller point cloud depths. In this way, by setting a reasonable weighted curvature threshold, points with a smaller point cloud depth and a larger curvature can be filtered out, thus retaining point cloud data with a clearer structure. Moreover, in different application scenarios, the weighted curvature threshold can also be adjusted adaptively. For example, for a scenario where the woods and bushes are denser nearby, the weighted curvature value can be set relatively small; while for a scenario where the woods and bushes are relatively sparse nearby, the weighted curvature value can be set relatively large.

[0119] In this embodiment, when fusing the point cloud data and the IMU data, the target IMU data located between the point cloud data frames can be selected from the IMU data for pose prediction to obtain the current predicted pose of the target area point cloud data; then the merged point cloud data is projected onto the coordinate system of the current predicted pose, and the geometric residuals of each point are calculated; finally, the merged point cloud data is iteratively updated based on the geometric residuals of the merged point cloud data to establish a point cloud map.

[0120] Specifically, first, between two frames of point cloud data, the target IMU data is pre-integrated to obtain a relative motion prediction. For example, a dynamics model can be used for prediction to obtain the current predicted pose of the point cloud data in the target area, expressed as:

[0121] x k = f(x k-1 , u k ) + w k (12)

[0122] where x k-1 is the prior state estimate, u k is the IMU data, which can be acceleration, angular velocity, etc., and w k is the process noise, which usually follows a Gaussian distribution.

[0123] In this process, a dynamics model can be used for prediction to obtain the current covariance prediction matrix, expressed as:

[0124] (13)

[0125] where F k is the state transition Jacobian matrix, Q k is the process noise covariance, P k is the current covariance prediction matrix, and P k-1 is the posterior covariance matrix at the previous moment (i.e., the covariance after update).

[0126] When projecting the merged point cloud data into the coordinate system of the current predicted pose, the geometric residual of each point can be expressed as:

[0127] (14)

[0128] where h(x k ) is the predicted pose under the current iteration state x k , and z k is the actual pose.

[0129] Subsequently, the Jacobian matrix is calculated based on the geometric residual of the merged point cloud data, expressed as:

[0130] (15)

[0131] And, based on the covariance prediction matrix and the Jacobian matrix, the current Kalman gain is determined, expressed as:

[0132] (15)

[0133] where R k is the observation noise covariance.

[0134] Finally, based on the geometric residual of the merged point cloud data and the current Kalman gain, the pose of the merged point cloud data is updated to obtain the updated current predicted pose. It is expressed as:

[0135] (16)

[0136] Generally, after a set number of iterations, the obtained current predicted pose can be used as the final output; alternatively, when the change in the residual is less than the set value, the obtained current predicted pose can be used as the final output.

[0137] The final output after the above iterative optimization can be used to insert the local point cloud into the global map and then used for the matching of subsequent frames. In specific steps, the point cloud can be transformed into the world coordinate system, and voxelization or ikd-Tree structure, etc. can be used for map update.

[0138] It can be seen that in the above process of fusing the reference point cloud data and IMU data, the high-frequency prediction of the IMU is first used to provide a short-term motion prior, and then combined with the geometric residual of the laser point cloud data to correct the long-term drift, realizing the complementarity of the IMU data and the point cloud data, and improving the accuracy and robustness of the inference.

[0139] Reference Figure 5 As shown in the mapping effect in a certain regional scene, it can be seen that in this scene, the woods and shrubs are dense, there is a bridge in the middle, and the scene is single. The mapping is successfully realized by using the method for establishing a point cloud map provided in the embodiments of the present application.

[0140] Refer Figure 6 , an embodiment of the device for establishing a point cloud map of the present application is introduced. In this embodiment, the device for establishing a point cloud map includes an acquisition module 21, a first calculation module 22, a second calculation module 23, and a map establishment module 24.

[0141] The acquisition module 21 is used to acquire the point cloud data and IMU data of the target area and determine the neighborhood space of each point in the point cloud data; the first calculation module 22 is used to calculate the covariance matrix of the point cloud in the neighborhood space and decompose it to obtain the corresponding eigenvalues; the second calculation module 23 is used to calculate the weighted curvature of the corresponding point based on the eigenvalues, the point cloud depth and geometric residual of the corresponding point; the map establishment module 24 is used to screen the point cloud data based on the weighted curvature and fuse the obtained reference point cloud data with the IMU data to establish a point cloud map.

[0142] In one embodiment, the map building module 24 is specifically configured to: construct a first function sub-item based on the eigenvalue, where the first function sub-item represents the curvature of the corresponding point; construct a second function sub-item based on the point cloud depth and geometric residual of the corresponding point, where the value of the second function sub-item is inversely correlated with the value of the point cloud depth; calculate the weighted curvature of the corresponding point based on the first function sub-item and the second function sub-item.

[0143] In one embodiment, the second function sub-item includes a proportional function of the point cloud depth and geometric residual of the corresponding point.

[0144] In one embodiment, the acquisition module 21 is specifically configured to: divide the point cloud data into ground point cloud data and non-ground point cloud data based on the elevation information of the point cloud data; determine the neighborhood space of each point in the non-ground point cloud data;

[0145] The map building module 24 is specifically configured to: screen the non-ground point cloud data based on the weighted curvature to obtain reference point cloud data; merge the reference point cloud data and the ground point cloud data, and fuse the obtained merged point cloud data with the IMU data to establish a point cloud map.

[0146] In one embodiment, the map building module 24 is specifically configured to: select target IMU data located between point cloud data frames from the IMU data for pose prediction to obtain the current predicted pose of the target area point cloud data; project the merged point cloud data into the coordinate system of the current predicted pose, and calculate the geometric residual of each point therein; iteratively update the merged point cloud data based on the geometric residual of the merged point cloud data to establish a point cloud map.

[0147] In one embodiment, the map building module 24 is specifically configured to: predict the current covariance prediction matrix based on the target IMU data, and calculate the Jacobian matrix based on the geometric residual of the merged point cloud data; determine the current Kalman gain based on the covariance prediction matrix and the Jacobian matrix; update the pose of the merged point cloud data based on the current Kalman gain and the geometric residual of the merged point cloud data to obtain the updated current predicted pose.

[0148] In one embodiment, the second calculation module 23 is specifically configured to: fit a reference plane and / or a reference line based on the point cloud within the neighborhood space; calculate the distance from the corresponding point to the reference plane and / or the reference line, and use it as the geometric residual.

[0149] As described above with reference to Figures 1 to 5, the method for establishing a point cloud map according to the embodiments of the present specification is described. The details mentioned in the above description of the method embodiments also apply to the device for establishing a point cloud map according to the embodiments of the present specification. The above device for establishing a point cloud map can be implemented in hardware, or can be implemented by software or a combination of hardware and software.

[0150] Figure 7 FIG. shows a hardware structure diagram of an autonomous vehicle according to an embodiment of the present specification. As Figure 7 shown, the autonomous vehicle 30 may include at least one processor 31, a memory 32 (such as a non-volatile memory), a memory 33, and a communication interface 34, and at least one processor 31, the memory 32, the memory 33, and the communication interface 34 are connected together via an internal bus 35. At least one processor 31 executes at least one computer-readable instruction stored or encoded in the memory 32.

[0151] It should be understood that the computer-executable instructions stored in the memory 32, when executed, cause at least one processor 31 to perform the various operations and functions described above in the various embodiments of the present specification in combination with Figures 1 to 5 description.

[0152] In the embodiments of the present specification, the autonomous vehicle 30 may be configured with a functional terminal to carry the above hardware structure, and the terminal may include, but is not limited to: a personal computer, a server computer, a workstation, a desktop computer, a laptop computer, a notebook computer, a mobile electronic device, a smart phone, a tablet computer, a cellular phone, a personal digital assistant (PDA), a handheld device, a messaging device, a wearable electronic device, a consumer electronic device, and the like.

[0153] According to one embodiment, a program product such as a machine-readable medium is provided. The machine-readable medium may have instructions (i.e., the above elements implemented in software form), which when executed by the machine, cause the machine to perform the various operations and functions described above in the various embodiments of the present specification in combination with Figures 1 - 5 description. Specifically, a system or device equipped with a readable storage medium may be provided, on which software program code for implementing the functions of any one of the above embodiments is stored, and the computer or processor of the system or device is caused to read and execute the instructions stored in the readable storage medium.

[0154] In this case, the program code read from the readable medium itself can implement the functions of any one of the above embodiments, so the machine-readable code and the readable storage medium storing the machine-readable code constitute a part of the present specification.

[0155] Examples of readable storage media include floppy disks, hard disks, magneto-optical disks, optical disks (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD-RW), magnetic tapes, non-volatile memory cards, and ROM. Optionally, program code can be downloaded from a server computer or the cloud via a communication network.

[0156] Those skilled in the art should understand that various modifications and variations can be made to the above-disclosed embodiments without departing from the essence of the invention. Therefore, the scope of protection of this specification should be defined by the appended claims.

[0157] It should be noted that not all steps and units in the above-mentioned processes and system structure diagrams are necessary, and some steps or units can be ignored according to actual needs. The execution order of each step is not fixed and can be determined as required. The device structures described in the above embodiments can be physical structures or logical structures. That is, some units may be implemented by the same physical entity, or some units may be implemented separately by multiple physical entities, or some components in multiple independent devices may be jointly implemented.

[0158] In the above embodiments, the hardware units or modules can be implemented mechanically or electrically. For example, a hardware unit, module, or processor can include permanent dedicated circuits or logic (such as a dedicated processor, FPGA, or ASIC) to perform corresponding operations. The hardware unit or processor can also include programmable logic or circuits (such as a general-purpose processor or other programmable processors), which can be temporarily set by software to perform corresponding operations. The specific implementation method (mechanical method, or dedicated permanent circuit, or temporarily set circuit) can be determined based on cost and time considerations.

[0159] The specific embodiments described above in conjunction with the accompanying drawings describe exemplary embodiments, but do not represent all embodiments that can be implemented or fall within the scope of protection of the claims. The term "exemplary" used throughout this specification means "serving as an example, instance, or illustration", and does not mean "preferred" or "advantageous" compared to other embodiments. For the purpose of providing an understanding of the described technology, the specific embodiments include specific details. However, these technologies can be implemented without these specific details. In some instances, well-known structures and devices are shown in block diagram form to avoid obscuring the concepts of the described embodiments.

[0160] The foregoing description of the disclosure is provided to enable any person of ordinary skill in the art to make or use the disclosure. Various modifications to the disclosure will be readily apparent to those of ordinary skill in the art, and the generic principles herein can be applied to other variations without departing from the scope of the disclosure. Thus, the disclosure is not limited to the examples and designs described herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for establishing a point cloud map, characterized in that, The method includes: Obtaining the point cloud data of the target area and the IMU data of the target vehicle, and determining the neighborhood space of each point in the point cloud data; Calculating the covariance matrix of the point cloud in the neighborhood space and decomposing it to obtain the corresponding eigenvalues; Calculating the weighted curvature of the corresponding point based on the eigenvalue, the point cloud depth of the corresponding point, and the geometric residual; Filtering the point cloud data based on the weighted curvature, and fusing the obtained reference point cloud data with the IMU data to establish a point cloud map.

2. The method for establishing a point cloud map according to claim 1, wherein Calculating the weighted curvature of the corresponding point based on the eigenvalue, the point cloud depth of the corresponding point, and the geometric residual, specifically including: Constructing a first function sub-item based on the eigenvalue, where the first function sub-item represents the curvature of the corresponding point; Constructing a second function sub-item based on the point cloud depth and geometric residual of the corresponding point, where the value of the second function sub-item is inversely correlated with the value of the point cloud depth; Calculating the weighted curvature of the corresponding point based on the first function sub-item and the second function sub-item.

3. The method for establishing a point cloud map according to claim 2, wherein, The second function sub-item includes a proportional function of the point cloud depth and geometric residual of the corresponding point.

4. The method for establishing a point cloud map according to claim 1, wherein Obtaining the point cloud data of the target area and the IMU data of the target vehicle, and determining the neighborhood space of each point in the point cloud data, specifically including: Dividing the point cloud data into ground point cloud data and non-ground point cloud data based on the elevation information of the point cloud data; Determining the neighborhood space of each point in the non-ground point cloud data; Filtering the point cloud data based on the weighted curvature, and fusing the obtained reference point cloud data with the IMU data to establish a point cloud map, specifically including: Filtering the non-ground point cloud data based on the weighted curvature to obtain reference point cloud data; Merging the reference point cloud data and the ground point cloud data, and fusing the obtained merged point cloud data with the IMU data to establish a point cloud map.

5. The method for establishing a point cloud map according to claim 4, characterized in that, Fusing the obtained reference point cloud data with the IMU data to establish a point cloud map, specifically including: Selecting target IMU data between point cloud data frames from the IMU data for pose prediction to obtain the current predicted pose of the point cloud data in the target area; Projecting the merged point cloud data into the coordinate system of the current predicted pose and calculating the geometric residual of each point therein; Iteratively updating the merged point cloud data based on the geometric residual of the merged point cloud data to establish a point cloud map.

6. The method for establishing a point cloud map according to claim 5, wherein The method specifically includes: Predicting the current covariance prediction matrix based on the target IMU data, and calculating the Jacobian matrix based on the geometric residual of the merged point cloud data; Determining the current Kalman gain based on the covariance prediction matrix and the Jacobian matrix; Updating the pose of the merged point cloud data based on the current Kalman gain and the geometric residual of the merged point cloud data to obtain the updated current predicted pose.

7. The method for establishing a point cloud map according to claim 1, wherein The method specifically includes: Fitting a reference plane and / or a reference line based on the point cloud in the neighborhood space; Calculating the distance from the corresponding point to the reference plane and / or the reference line and using it as the geometric residual.

8. An apparatus for establishing a point cloud map, characterized in that, Includes: An acquisition module, configured to acquire point cloud data and IMU data of a target area, and determine the neighborhood space of each point in the point cloud data; A first calculation module, configured to calculate the covariance matrix of the point cloud in the neighborhood space and decompose it to obtain the corresponding eigenvalues; A second calculation module, configured to calculate the weighted curvature of a corresponding point based on the eigenvalue, as well as the point cloud depth and geometric residual of the corresponding point; A map building module, configured to filter the point cloud data based on the weighted curvature, and fuse the obtained reference point cloud data with the IMU data to build a point cloud map.

9. An autonomous vehicle, characterized in that, Comprising: At least one processor; And A memory, storing instructions that, when executed by the at least one processor, cause the at least one processor to execute the method for building a point cloud map according to any one of claims 1 to 7.

10. A machine-readable storage medium storing executable instructions, characterized in that, When executed, the instructions cause the machine to execute the method for building a point cloud map according to any one of claims 1 to 7.

Images