Point cloud map establishment method and device, unmanned vehicle and storage medium

Abstract

This application discloses a method and device for establishing a point cloud map, an unmanned vehicle, and a storage medium. The method includes acquiring point cloud data of a target area and IMU data of a 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 points based on the eigenvalues, the point cloud depth, and the geometric residual of the corresponding points; 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. In this way, mapping can be achieved in complex scenes, especially in environments including dense forests and bushes, and the mapping efficiency is high.

Description

Technical Field

[0001] The present application belongs to the field of autonomous driving technology, and specifically relates to a method and device for establishing a point cloud map, an unmanned vehicle, and a storage medium. Background Art

[0002] At present, high-precision maps are being used more and more widely in autonomous driving. In some complex scenarios, such as environments with dense woods and bushes, existing mapping methods often cannot meet the needs.

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

[0004] The purpose of this application is to provide a method for establishing a point cloud map, which is used to solve the problem that existing mapping methods cannot meet the mapping needs of complex scenes including dense forests and bushes.

[0005] To achieve the above objectives, the present application provides a method for establishing a point cloud map, the method comprising:

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

[0007] Calculating the covariance matrix of the point cloud in the neighborhood space and decomposing it to obtain corresponding eigenvalues;

[0008] Calculating the weighted curvature of the corresponding point based on the eigenvalue, the point cloud depth and the geometric residual of the corresponding point;

[0009] The point cloud data is filtered based on the weighted curvature, and the obtained reference point cloud data is fused with the IMU data to establish a point cloud map.

[0010] In one embodiment, the weighted curvature of the corresponding point is calculated based on the eigenvalue, the point cloud depth and the geometric residual of the corresponding point, and specifically includes:

[0011] constructing a first function sub-item based on the eigenvalue, wherein the first function sub-item represents the curvature of the corresponding point;

[0012] Constructing a second function sub-item based on the point cloud depth and the geometric residual of the corresponding point, wherein the value of the second function sub-item is inversely correlated with the value of the point cloud depth;

[0013] Based on the first function sub-term and the second function sub-term, weighted curvatures of corresponding points are calculated.

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

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

[0016] 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;

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

[0018] The point cloud data is filtered based on the weighted curvature, and the reference point cloud data is fused with the IMU data to establish a point cloud map, specifically including:

[0019] Filtering the non-ground point cloud data based on the weighted curvature to obtain reference point cloud data;

[0020] 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 create a point cloud map.

[0021] In one embodiment, the reference point cloud data obtained is fused with the IMU data to create 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 to the coordinate system of the current predicted pose and calculating the geometric residual of each point therein;

[0024] The merged point cloud data is iteratively updated based on the geometric residual of the merged point cloud data to establish a point cloud map.

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

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

[0027] Determining a current Kalman gain based on the covariance prediction matrix and the Jacobian matrix;

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

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

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

[0031] The distance between the corresponding point and the reference plane and / or reference line is calculated and used as the geometric residual.

[0032] This application also provides a device for creating a point cloud map, comprising:

[0033] An acquisition module is used to acquire 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 computing module is used to calculate the covariance matrix of the point cloud in the neighborhood space and decompose it to obtain corresponding eigenvalues;

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

[0036] A map building module is used 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.

[0037] The present application also provides an unmanned vehicle, comprising:

[0038] at least one processor; and

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

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

[0041] Compared with the existing technology, according to the point cloud map establishment method of the present application, by determining the neighborhood space of each point in the point cloud data in the target area, and then calculating the covariance matrix of the point cloud in the neighborhood space respectively, to decompose and obtain the corresponding eigenvalues, and then the weighted curvature of the corresponding point can be calculated based on the eigenvalues ​​and 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 realize mapping, which improves the success rate of mapping in complex environments, especially scenes including dense woods and bushes, and has a high mapping efficiency.

[0042] On the other hand, when fusing IMU data and point cloud data, the 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 to the coordinate system of the predicted pose, and the geometric residual of each point therein is 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, and the high-frequency prediction based on the IMU is used to provide short-term motion priors, which are then combined with the geometric residuals of the laser point cloud data to correct long-term drift, thereby achieving the complementarity of IMU data and point cloud data and improving the accuracy and robustness of reasoning. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 This 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 is a flowchart of a method for establishing a point cloud map according to an embodiment of the present application;

[0045] Figure 3 A scene graph for screening point clouds in a method for establishing a point cloud map according to an embodiment of the present application;

[0046] Figure 4 This is a schematic diagram of a process for 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 This 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 A module diagram of a device for creating a point cloud map according to an embodiment of the present application;

[0049] Figure 7 This is a hardware structure diagram of an unmanned vehicle according to an embodiment of the present application. DETAILED DESCRIPTION

[0050] The present application will be described in detail below with reference to the various embodiments shown in the accompanying drawings. However, these embodiments do not limit the present application, and any structural, methodological, or functional modifications made by a person skilled in the art based on these embodiments are included within the scope of protection of the present application.

[0051] The terms "first," "second," "third," "fourth," and the like (if any) in the specification and claims of this application and in the accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a particular order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate, so that the embodiments of the application described herein can, for example, be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "corresponding to," and any variations thereof, are intended to cover non-exclusive inclusions. For example, a process, method, system, product, or apparatus comprising a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such process, method, product, or apparatus.

[0052] Before introducing the embodiments of the present application, the basic technologies and some technical terms involved in the embodiments of the present application are schematically explained:

[0053] Autonomous driving refers to the ability to guide and make decisions about vehicle driving tasks without the need for a test driver to physically perform driving operations, and to control the vehicle safely on behalf of the test driver. Autonomous driving technology typically includes high-precision mapping, environmental perception, behavioral decision-making, path planning, motion control, and other technologies.

[0054] Autonomous driving system: A system that implements different levels of autonomous driving functions for a vehicle, such as assisted driving system (L2), high-speed autonomous driving system requiring human supervision (L3), and highly / fully autonomous driving system (L4 / L5).

[0055] Point cloud data refers to a collection of vectors in a three-dimensional coordinate system. This collection is recorded as points, each of which contains three-dimensional coordinates and can carry additional information about its properties, such as color, reflectivity, and intensity. Point cloud data is typically acquired by devices such as laser scanners, cameras, and 3D scanners and can be used in applications such as 3D modeling, scene reconstruction, robotic navigation, and virtual and augmented reality.

[0056] Point cloud data is characterized by high precision, high resolution, and high-dimensional geometric information, which can intuitively represent the shape, surface, and texture of objects in space. The processing and analysis of point cloud data typically requires the use of computer vision and computer graphics techniques, such as point cloud filtering, registration, segmentation, reconstruction, recognition, and classification.

[0057] IMU data (Inertial Measurement Unit data) is obtained from sensors such as accelerometers, gyroscopes, and magnetometers. IMU data typically includes acceleration and angular velocity data and is used in conjunction with other sensors (such as GNSS and 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 embodiment of this application. Figure 1 As shown in the figure, the lidar collects point cloud data of the current environment, and the inertial measurement unit collects 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 the server receives the point cloud data and IMU data, it 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 vehicle-mounted terminals and user terminals.

[0059] In the above architecture, the lidar can be installed on a roadside device or a radar on a vehicle, the inertial measurement unit can be installed directly on the vehicle body (for example, the center of mass), and the server can be a third-party server, such as a company's server, which is used to create point cloud maps of the target area based on the data collected by the lidar and inertial measurement unit. The server can also be an on-board device. For example, if an unmanned vehicle comes with a map, the on-board device can directly create point cloud maps based on the data collected by the lidar and inertial measurement unit.

[0060] Continue to participate Figure 1 , a specific scenario in which the method for establishing a point cloud map of the present application is applied may include a server and a terminal device. Among them, the terminal device may include a vehicle-mounted terminal and a user terminal. The vehicle-mounted terminal may include a driving computer or an on-board unit (OBU), etc. The vehicle-mounted terminal may also be an application (APP) on the terminal, an APP on the smart rearview mirror, an APP or a mini-program on a mobile phone, etc., which are not limited here. The user terminal (user equipment, UE) may be a wireless terminal device or a wired terminal device. The wireless terminal device may refer to a device with wireless transceiver function. The user terminal may be a mobile phone (mobile phone), a tablet computer (Pad), a computer with wireless transceiver function, a virtual reality (VR) user device, an augmented reality (AR) user device, an intelligent voice interaction device, a smart home appliance, a vehicle-mounted terminal, an aircraft, etc., which are not limited here.

[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. Acquire point cloud data of the target area and IMU data of the target vehicle, and determine the neighborhood space of each point in the point cloud data.

[0068] With ginseng Figure 4 The point cloud data can be a single-frame point cloud obtained by the laser radar at a certain moment, or a collection of multi-frame point clouds obtained by the laser radar periodically scanning within a set period. The point cloud map establishment method provided in each embodiment of the present application can be based on a single-frame point cloud or a collection of multi-frame point clouds. The target area can be the coverage of the point cloud data scanned by the laser radar. Correspondingly, if the laser radar is installed on an unmanned vehicle, the multi-frame point cloud obtained by the unmanned vehicle during the driving process can include point cloud data of a larger range than that of a single-frame point cloud. This application does not limit the range or scanning time of the target area.

[0069] IMU data corresponds to point cloud data, and the target vehicle acquires IMU data simultaneously with point cloud data. Point cloud data typically has a relatively low frequency, such as 10 Hz, while IMU data typically has a relatively high frequency, such as 100 Hz.

[0070] The neighborhood space of each point in the point cloud data can be determined in a variety of ways. For example, a space with a certain range can be determined with the corresponding point as the center. In another example, the range of the neighborhood space can be determined by adjacent points in the point cloud, calculating N point cloud data adjacent to the corresponding point, and using the range defined by these N point cloud data as the neighborhood space corresponding to the point. The neighborhood spaces corresponding to different points may or may not overlap, and this application does not impose any restrictions on this.

[0071] In one embodiment, a radius search method can be used to determine the neighborhood space. By searching for neighboring points whose distance to the corresponding point is less than a given radius (e.g., 300 meters), a set of neighboring points is determined and used as the neighborhood space of the corresponding point. In another embodiment, a K-nearest neighbor method can be used to determine the neighborhood space. By searching for the K points closest to the corresponding point, a set of neighboring points is determined and used as the neighborhood space of the corresponding point.

[0072] In this step, the determination of the neighborhood space of a point in the point cloud is primarily used for subsequent screening of point clouds that are not trees or bushes. In actual vehicle driving scenarios, point cloud data may also include information such as building walls, roadside light poles, and road surfaces. Road surfaces typically have lower elevations than other objects. Therefore, this embodiment further proposes 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, and only determining the neighborhood space of each point in the non-ground point cloud data. By assuming that point clouds with elevations below a set threshold are ground point clouds and using this to distinguish between ground point clouds and non-ground point clouds, the screening range of non-tree and bush point clouds can be reduced, speeding up mapping.

[0073] The threshold for filtering non-ground point clouds can be determined based on the actual terrain and the target area's scope. In situations where the terrain's elevation fluctuates significantly, the threshold can be increased accordingly. The larger the target area, the larger the threshold can be set, for example, to 0.3m, 0.4m, 0.5m, etc. Alternatively, the target vehicle's body can be directly factored in, with the threshold set to the vehicle height minus 0.3m, 0.4m, etc.

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

[0075] S13. Calculate the weighted curvature of the corresponding point based on the eigenvalue, the point cloud depth, and the geometric residual of the corresponding point.

[0076] When calculating the covariance matrix of the point cloud in the neighborhood space, the original point cloud data can also be preprocessed, such as removing motion distortion, voxel filtering and downsampling. 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, P i is the i-th point in the neighborhood space.

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

[0080] (2)

[0081] Where Λ=diag(λ1,λ2,λ3), and 0≤λ1≤λ2≤λ3, λ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, indicating the principal component directions of the data.

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

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

[0084]

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

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

[0087] (3)

[0088] In formula (3), σ i is the curvature of the i-th point in the point cloud data. It is understood that the first function sub-item represents the curvature of the corresponding point, and is not limited to representing the actual value of the curvature of the corresponding point. In different embodiments, the first function sub-item can also represent the relative size of the curvature of the corresponding point. For example, the first function sub-item can also be expressed as:

[0089] (4)

[0090] Wherein, m is a tuning parameter set to be greater than or equal to 0.

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

[0092] The geometric residual of the corresponding points can be point-to-plane residual or point-to-line residual. Point-to-plane residual is suitable for environments with rich plane features, such as walls and floors; point-to-line residual is suitable for environments with rich edge or linear structures, such as door frames, corridors, telephone poles, tree trunks, etc. In actual applications, one or a combination of point-to-plane residual and point-to-line residual can be selected according to the characteristics of the environment.

[0093] Any point P i The point-surface residual can be expressed as:

[0094] (5)

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

[0096] Any point P i The dotted residual of can be expressed as:

[0097] (6)

[0098] Where d is the direction vector of the fitted line in the local map corresponding to the neighborhood space, and q is any reference point on the fitted line, such as the center point of the point cloud in the neighborhood space.

[0099] The geometric residual when considering both point-surface residual and point-line residual can be expressed as:

[0100] (7)

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

[0102] The second function sub-item can be used to set a proportional function between the point cloud depth and the geometric residual of the corresponding point. 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 actual application scenarios, the geometric residual fd of the corresponding point is usually smaller than the point cloud depth depth of the corresponding point, for example, fd = 4mm, depth = 200m. Using this proportional function, a second function sub-item value with a larger range of variation can be obtained, providing a more sensitive curvature reference for the subsequent setting of the weighted curvature screening threshold, which is conducive to screening out point cloud data with clearer structure. It is understandable that in some embodiments, the second function sub-item can also be constructed as:

[0106] (9)

[0107] It is also understandable that, in order to construct a second function sub-item that is inversely correlated with the point cloud depth value, the second function sub-item may include, in addition to a function representing the ratio of the point cloud depth and the geometric residual of the corresponding point, various other reasonable forms, such as an inverse log function, may also be employed. The weighted curvature screening threshold may also be adjusted accordingly to different second function sub-item forms, and this application does not impose any limitations thereto.

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

[0109] (10)

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

[0111] (11)

[0112] S14. Filter 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.

[0113] In this embodiment, non-ground point cloud data can be screened 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] For example, taking equation (10) as an example, it is assumed that the non-ground point cloud data includes points A, B, and C.

[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] The weighted curvatures of points A, B, and C are calculated to be 0.00333, 0.00298, and 0.00044, respectively. It can be seen that the weighted curvature at point C, where the point cloud depth is greater, is an order of magnitude different from the weighted curvatures at points A and B, where the point cloud depth is smaller. Thus, by setting a reasonable weighted curvature threshold, points with smaller point cloud depth and larger curvature can be filtered out, thereby retaining point cloud data with clearer structure. Furthermore, the weighted curvature threshold can be adaptively adjusted in different application scenarios. For example, for scenes with dense nearby trees and bushes, the weighted curvature value can be set relatively small; whereas for scenes with relatively sparse nearby trees and bushes, the weighted curvature value can be set relatively large.

[0119] In this embodiment, when fusing point cloud data and 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; the merged point cloud data is then projected to the coordinate system of the current predicted pose, and the geometric residual of each point therein is calculated; finally, the merged point cloud data is iteratively updated based on the geometric residual of the merged point cloud data to establish a point cloud map.

[0120] Specifically, we first pre-integrate the target IMU data between two frames of point cloud data to obtain a relative motion prediction. For example, we can use a dynamic model to predict the current predicted pose of the target area point cloud data, which can be expressed as:

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

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

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

[0124] (13)

[0125] Among them, F k is the state transfer Jacobian matrix, Q k is the process noise covariance, P k is the current covariance prediction matrix, P k-1 is the posterior covariance matrix of the previous moment (i.e. the updated covariance).

[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] Among them, h(x k ) is the current iteration state x k The predicted pose under z k is the actual posture.

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

[0130] (15)

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

[0132] (15)

[0133] Among them, 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 can be expressed as:

[0135] (16)

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

[0137] The final output of the above iterative optimization can be inserted into the global map as a local point cloud and then used for matching in subsequent frames. Specifically, the point cloud can be converted to the world coordinate system and the map can be updated using voxelization or the ikd-Tree structure.

[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 short-term motion priors, and then the geometric residuals of the laser point cloud data are combined to correct the long-term drift, thereby achieving the complementarity of the IMU data and the point cloud data and improving the accuracy and robustness of the reasoning.

[0139] refer to Figure 5 The mapping effect of a certain area scene is shown. It can be seen that the scene is dense with trees and shrubs, with a bridge in the middle, and the scene is simple. The mapping is successfully achieved by using the point cloud map creation method provided in the embodiment of the application.

[0140] Ginseng Figure 6 , an embodiment of the point cloud map creation device of the present application is introduced. In this embodiment, the point cloud map creation device includes an acquisition module 21, a first calculation module 22, a second calculation module 23, and a map creation 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 filter 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 establishment module 24 is specifically used to: construct a first function sub-item based on the eigenvalue, wherein 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, wherein the value of the second function sub-item is inversely correlated with the value of the point cloud depth; and 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 ratio function of the point cloud depth and the 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 used to: filter 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 establishment module 24 is specifically used to: select the target IMU data located between the 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 to 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 establishment module 24 is specifically used 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; and 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 an updated current predicted pose.

[0148] In one embodiment, the second calculation module 23 is specifically used to: fit a reference plane and / or a reference line based on the point cloud in 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 above Figures 1 to 5, a method for establishing a point cloud map according to an embodiment of this specification is described. The details mentioned in the above description of the method embodiment are also applicable to the device for establishing a point cloud map according to the embodiment of this specification. The above device for establishing a point cloud map can be implemented using hardware, software, or a combination of hardware and software.

[0150] Figure 7 FIG1 shows a hardware structure diagram of an unmanned vehicle according to an embodiment of this specification. Figure 7 As shown, the unmanned vehicle 30 may include at least one processor 31, a memory 32 (e.g., a non-volatile memory), a memory 33, and a communication interface 34, and the at least one processor 31, the memory 32, the memory 33, and the communication interface 34 are connected together via an internal bus 35. The 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 above combined operations in various embodiments of this specification. Figures 1 to 5 Describes the various operations and functions.

[0152] In the embodiments of the present specification, the unmanned vehicle 30 can be configured with a functional terminal to carry the above-mentioned 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 elements implemented in the form of software) that, when executed by a machine, cause the machine to perform the above-mentioned combined operations in various embodiments of this specification. Figure 1-Figure 5 Specifically, a system or device equipped with a readable storage medium can be provided, on which software program codes for implementing the functions of any of the above-mentioned embodiments are stored, and a computer or processor of the system or device can be enabled to read and execute the instructions stored in the readable storage medium.

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

[0155] Examples of readable storage media include floppy disks, hard disks, magneto-optical disks, optical disks (e.g., CD-ROMs, CD-Rs, CD-RWs, DVD-ROMs, DVD-RAMs, DVD-RWs, DVD-RWs), magnetic tapes, non-volatile memory cards, and ROMs. Alternatively, the program code may be downloaded from a server computer or a cloud via a communication network.

[0156] Those skilled in the art will appreciate that the various embodiments disclosed above may be modified and altered in various ways without departing from the essence of the invention. Therefore, the scope of protection of this specification shall be defined by the appended claims.

[0157] It should be noted that not all steps and units in the above processes and system structure diagrams are required, and certain steps or units can be omitted according to actual needs. The execution order of each step is not fixed and can be determined as needed. The device structure described in the above embodiments can be a physical structure or a logical structure, that is, some units may be implemented by the same physical client, or some units may be implemented by multiple physical clients, or may be implemented by certain components in multiple independent devices.

[0158] In the above embodiments, the hardware unit or module can be implemented mechanically or electrically. For example, a hardware unit, module, or processor may include permanent dedicated circuits or logic (such as a dedicated processor, FPGA, or ASIC) to complete the corresponding operation. The hardware unit or processor may also include programmable logic or circuits (such as a general-purpose processor or other programmable processor), which can be temporarily configured by software to complete the corresponding operation. The specific implementation method (mechanical method, dedicated permanent circuit, or temporarily configured 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 "used as an example, instance or illustration" and does not mean "preferred" or "having advantages" over 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, in order to avoid obscuring the concepts of the described embodiments, well-known structures and devices are shown in block diagram form.

[0160] The foregoing description of the present disclosure is provided to enable any person skilled in the art to implement or use the present disclosure. Various modifications to the present disclosure will be readily apparent to those skilled in the art, and the general principles herein may be applied to other variations without departing from the scope of the present disclosure. Therefore, the present disclosure is not limited to the examples and designs described herein, but is intended to be consistent with the widest range of principles and novel features disclosed herein.

Claims

1. A method for establishing a point cloud map, characterized in that: The method comprises: Obtaining point cloud data of the target area and 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 corresponding eigenvalues; Calculating the weighted curvature of the corresponding point based on the eigenvalues ​​and the point cloud depth and geometric residual of the corresponding point, specifically comprising constructing a first function sub-item based on the eigenvalues, wherein 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 the geometric residual of the corresponding point, wherein the value of the second function sub-item is inversely correlated with the value of the point cloud depth; and calculating the weighted curvature of the corresponding point based on the first function sub-item and the second function sub-item; The point cloud data is filtered based on the weighted curvature, and the obtained reference point cloud data is fused with the IMU data to establish a point cloud map.

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

3. The method for establishing a point cloud map according to claim 1, wherein: Obtaining point cloud data of the target area and IMU data of the target vehicle, and determining the neighborhood space of each point in the point cloud data, specifically including: 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; Determining a neighborhood space of each point in the non-ground point cloud data; The point cloud data is filtered based on the weighted curvature, and the reference point cloud data is fused 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; 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 create a point cloud map.

4. The method for establishing a point cloud map according to claim 3, wherein: The reference point cloud data obtained is fused with the IMU data to create a point cloud map, specifically including: 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; Projecting the merged point cloud data to the coordinate system of the current predicted pose and calculating the geometric residual of each point therein; The merged point cloud data is iteratively updated based on the geometric residual of the merged point cloud data to establish a point cloud map.

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

6. 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; The distance between the corresponding point and the reference plane and / or reference line is calculated and used as the geometric residual.

7. A device for establishing a point cloud map, characterized in that: include: An acquisition module is used to acquire point cloud data and IMU data of the target area and determine the neighborhood space of each point in the point cloud data; A first computing module is used to calculate the covariance matrix of the point cloud in the neighborhood space and decompose it to obtain corresponding eigenvalues; a second calculation module, configured to calculate a weighted curvature of the corresponding point based on the eigenvalue, and the point cloud depth and geometric residual of the corresponding point, specifically configured to construct a first function sub-item based on the eigenvalue, wherein 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 the geometric residual of the corresponding point, wherein the value of the second function sub-item is inversely correlated with the value of the point cloud depth; and calculate the weighted curvature of the corresponding point based on the first function sub-item and the second function sub-item; A map building module is used 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.

8. An unmanned vehicle, characterized in that: include: at least one processor; as well as A memory storing instructions, which, when executed by the at least one processor, causes the at least one processor to execute the method for establishing a point cloud map according to any one of claims 1 to 6.

9. A machine-readable storage medium storing executable instructions, characterized in that: When the instructions are executed, the machine executes the method for establishing a point cloud map as described in any one of claims 1 to 6.

Images