Three-dimensional map construction method, device and system

Abstract

The invention provides a three-dimensional map construction method, device and system, and relates to the field of engineering machinery intelligence. Based on the point cloud data of the laser radar, the three-dimensional map point cloud around the excavator can be constructed, and the normal distribution information of each voxel of the local map point cloud is updated by using the container, so that the transformation matrix from the laser radar coordinate system to the excavator getting-off coordinate system is determined; and voxelization of the local map point cloud and recalculation of the normal distribution information of the voxel based on the point cloud points in the voxel in each point cloud registration are not needed, so that the time consumption is reduced, and the three-dimensional map point cloud around the excavator can be quickly and efficiently constructed.

Description

Technical Field

[0001] The present disclosure relates to the field of intelligent engineering machinery, and in particular to a three-dimensional map construction method, device and system. Background Art

[0002] Constructing a 3D map of earthwork material areas is one of the key technologies in many fields such as engineering construction, geological exploration, and resource management. Traditional earthwork material area measurement methods mainly rely on manual measurement or single-point measurement instruments such as total stations. These methods are not only time-consuming and labor-intensive, but also have limited accuracy and are difficult to fully and accurately reflect the true shape and volume of earthwork materials.

[0003] The intelligentization of construction machinery has become the future trend of the industry. As an important field of construction machinery, excavators are widely used in construction fields such as construction and mining. In the intelligent operation of excavators, excavators need to obtain three-dimensional information of materials around the construction work to provide a good data foundation for subsequent excavator perception, planning and control processes. Therefore, how to quickly and efficiently obtain three-dimensional image information of earthwork materials has become an urgent problem to be solved. Summary of the invention

[0004] The disclosed embodiment is based on the point cloud data of the laser radar, which can not only construct a three-dimensional map point cloud around the excavator, but also use the container to update the normal distribution information of each voxel of the local map point cloud, and determine the transformation matrix from the laser radar coordinate system to the excavator's off-vehicle coordinate system. There is no need to voxelize the local map point cloud and recalculate the normal distribution information of the voxels based on the point cloud points in the voxels for each point cloud registration, which reduces time consumption and can quickly and efficiently construct a three-dimensional map point cloud around the excavator.

[0005] Some embodiments of the present disclosure provide a three-dimensional map construction method, including:

[0006] Based on the laser radar of the excavator, obtain the current frame point cloud in the laser radar coordinate system;

[0007] Obtain the normal distribution information of each voxel of the local map point cloud in the excavator disembarkation coordinate system recorded by the container;

[0008] According to the residual information of the normal distribution information of each voxel of the local map point cloud obtained by applying the transformation matrix from the laser radar coordinate system to the excavator off-vehicle coordinate system to the current frame point cloud, the transformation matrix is ​​updated to obtain the target transformation matrix;

[0009] The target transformation point cloud and the local map point cloud obtained by fusing the target transformation matrix to the current frame point cloud to update the local map point cloud;

[0010] For any voxel, the normal distribution information of the target transformation point cloud at the voxel and the normal distribution information of the local map point cloud recorded by the container at the voxel are fused to determine the updated normal distribution information of the local map point cloud at the voxel, and update it to the container;

[0011] By using the continuously acquired point clouds of each current frame, the local map point cloud and the records of the container are continuously updated to obtain a three-dimensional global map point cloud.

[0012] In some embodiments, it also includes:

[0013] Determine whether the current frame point cloud includes the excavation location point cloud;

[0014] For the non-excavation location point cloud, participating in executing the steps of claim 1;

[0015] For the excavation location point cloud, the transformation matrix update step and the normal distribution information fusion step are not executed. The normal distribution information of the same voxel in the local map point cloud recorded by the container as the excavation location point cloud is cleared and updated to the normal distribution information of the same voxel in the excavation location point cloud.

[0016] In some embodiments, updating the transformation matrix includes:

[0017] Construct the residual information of the coordinates of the transformed point cloud obtained by applying the transformation matrix to the coordinates of the point in the current frame relative to the mean of the normal distribution of the same voxel in the local map point cloud;

[0018] With the goal of reducing the residual information, the transformation matrix is ​​continuously updated iteratively to obtain the target transformation matrix.

[0019] In some embodiments, the initial transformation matrix in the transformation matrix update process is determined according to the excavator rotation angle information.

[0020] In some embodiments, the initial transformation matrix in the transformation matrix update process of the i+1th frame point cloud is for:

[0021]

[0022] Among them, θ i and θ i+1 are the corresponding excavator rotation angles when the i-th frame point cloud and the i+1-th frame point cloud are collected, z represents the physical offset between the excavator on-board coordinate system and the excavator off-board coordinate system, T i Represents the transformation matrix from the i-th frame point cloud to the excavator's off-vehicle coordinate system.

[0023] In some embodiments, for any voxel, fusing the normal distribution information of the target transformed point cloud at the voxel and the normal distribution information of the local map point cloud recorded by the container at the voxel to determine the updated normal distribution information of the local map point cloud at the voxel includes:

[0024] calculate

[0025] Among them, X represents the mean of the normal distribution of the updated local map point cloud in the voxel, X1 represents the mean of the normal distribution of the local map point cloud in the voxel, X2 represents the mean of the normal distribution of the target transformation point cloud in the voxel, m represents the number of points of the local map point cloud in the voxel, and n represents the number of points of the target transformation point cloud in the voxel.

[0026] In some embodiments, for any voxel, fusing the normal distribution information of the target transformed point cloud at the voxel and the normal distribution information of the local map point cloud recorded by the container at the voxel to determine the updated normal distribution information of the local map point cloud at the voxel includes:

[0027] calculate

[0028] Among them, X1 represents the mean of the normal distribution of the local map point cloud in the voxel, X2 represents the mean of the normal distribution of the target transformed point cloud in the voxel, m represents the number of points of the local map point cloud in the voxel, n represents the number of points of the target transformed point cloud in the voxel, σ represents the variance of the normal distribution of the updated local map point cloud in the voxel, σ1 represents the variance of the normal distribution of the local map point cloud in the voxel, and σ2 represents the variance of the normal distribution of the target transformed point cloud in the voxel.

[0029] In some embodiments, it also includes: using a statistical filtering algorithm to remove outlier noise points in the point cloud of the current frame.

[0030] In some embodiments, the method further includes: removing dust point clouds and / or dust point clouds in the current frame point cloud according to the point cloud reflection intensity information.

[0031] In some embodiments, it also includes: based on the unified robot description format model of the excavator, removing the excavator body point cloud from the current frame point cloud.

[0032] In some embodiments, the normal distribution information of a voxel includes: a mean and a variance of a normal distribution of a point cloud at the voxel, and the number of points of the point cloud at the voxel.

[0033] Some embodiments of the present disclosure provide a three-dimensional map construction device, including: a module for executing a three-dimensional map construction method.

[0034] Some embodiments of the present disclosure provide a three-dimensional map construction device, comprising: a memory; and a processor coupled to the memory, wherein the processor is configured to execute a three-dimensional map construction method based on instructions stored in the memory.

[0035] Some embodiments of the present disclosure provide a three-dimensional map construction system, including:

[0036] A laser radar installed on the excavator is configured to emit laser signals, collect reflected signals, and obtain point cloud data by calculating and processing the reflected signals; and

[0037] The three-dimensional map construction device is configured to execute a three-dimensional map construction method.

[0038] Some embodiments of the present disclosure provide a computer-readable storage medium having computer instructions stored thereon, which implement the steps of a three-dimensional map construction method when executed by a processor.

[0039] Some embodiments of the present disclosure provide a computer program product, including computer instructions, which implement the steps of a three-dimensional map construction method when executed by a processor. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The following is a brief introduction to the drawings required for use in the embodiments or related technical descriptions. The present disclosure can be more clearly understood according to the following detailed description with reference to the drawings.

[0041] Obviously, the drawings described below are only some embodiments of the present disclosure, and a person skilled in the art can obtain other drawings based on these drawings without creative work.

[0042] Figure 1 A schematic diagram showing a flow chart of a three-dimensional map construction method according to some embodiments of the present disclosure.

[0043] Figure 2 A schematic diagram of the preprocessing process of the lidar point cloud according to some embodiments of the present disclosure is shown.

[0044] Figure 3 A schematic diagram showing the principle of the normal distribution transformation algorithm.

[0045] Figure 4 A schematic diagram showing voxel updating during the mining process of some embodiments of the present disclosure is shown.

[0046] Figure 5 A schematic diagram showing the construction of a three-dimensional map according to this embodiment.

[0047] Figure 6 A schematic diagram showing the structure of a three-dimensional map construction device according to some embodiments of the present disclosure.

[0048] Figure 7 A schematic diagram showing the structure of a three-dimensional map construction device according to some embodiments of the present disclosure.

[0049] Figure 8 A schematic diagram showing the structure of a three-dimensional map construction system according to some embodiments of the present disclosure. DETAILED DESCRIPTION

[0050] It should be noted that the relative arrangement of components and steps, the numerical expressions and numerical values ​​set forth in these embodiments do not limit the scope of the present disclosure unless specifically stated otherwise.

[0051] Those skilled in the art can understand that the terms "first" and "second" in the embodiments of the present disclosure are only used to distinguish different steps, devices or modules, etc., and neither represent any specific technical meaning nor indicate the necessary logical order between them.

[0052] It should also be understood that in the embodiments of the present disclosure, “plurality” may refer to two or more than two, and “at least one” may refer to one, two, or more than two.

[0053] It should also be understood that any component, data or structure mentioned in the embodiments of the present disclosure can generally be understood as one or more, unless explicitly limited or otherwise indicated in the context.

[0054] In addition, the term "and / or" in the present disclosure is only a description of the association relationship of associated objects, indicating that three relationships may exist. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in the present disclosure generally indicates that the associated objects before and after are in an "or" relationship.

[0055] It should also be understood that the description of the various embodiments in the present disclosure focuses on the differences between the various embodiments, and the same or similar aspects thereof can be referenced to each other, and for the sake of brevity, they will not be described one by one.

[0056] At the same time, it should be understood that for the convenience of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship.

[0057] The following description of at least one exemplary embodiment is merely illustrative in nature and is in no way intended to limit the present disclosure, its application, or uses.

[0058] Technologies, methods, and equipment known to ordinary technicians in the relevant art may not be discussed in detail, but where appropriate, the technologies, methods, and equipment should be considered as part of the specification.

[0059] It should be noted that like reference numerals and letters refer to similar items in the following figures, and therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0060] In addition, in order to avoid obscuring the present disclosure due to unnecessary details, only the processing steps and / or device structures closely related to at least the scheme according to the present disclosure are shown in the drawings, and other details that are not closely related to the present disclosure are omitted. It should also be noted that similar reference numerals and letters in the drawings indicate similar items, and therefore once an item is defined in one drawing, it does not need to be discussed again for subsequent drawings.

[0061] The three-dimensional map construction method of the disclosed embodiment is applied to an excavator, for example. The excavator digs materials from a designated excavation area on a working platform, transfers to a material unloading area through a rotation action to unload the materials, and the above actions are repeated. The shape of the working platform is, for example, a trapezoid, with a height of about 2 meters, a slope of about 75°, and a width of about 1 meter from the outer edge of the excavator track. The area below the loading platform is the earthwork material area.

[0062] Point cloud data is usually obtained by 3D scanners, and LiDAR is one of them. LiDAR emits laser signals to the ground or target and collects the reflected signals. By calculating and processing these reflected signals, point cloud data can be obtained.

[0063] The disclosed embodiment uses a laser radar device to perform a three-dimensional scan of the surrounding environment of the excavator and obtain three-dimensional point cloud information of the surrounding environment. The laser radar device is a remote sensing technology that uses a laser beam to measure distance, shape, direction and speed. It calculates the distance of the target object by emitting laser pulses and measuring the time required for these pulses to be reflected by the target and return. The laser radar also has the ability to measure angles and speeds, and can generate three-dimensional coordinate information of the target to form a three-dimensional point cloud model of the environment.

[0064] Figure 1 A schematic diagram showing a flow chart of a three-dimensional map construction method of some embodiments of the present disclosure. The three-dimensional map construction method of this embodiment is executed, for example, by a three-dimensional map construction device. Figure 1 As shown, the three-dimensional map construction method of this embodiment includes the following steps.

[0065] In step 110, based on the laser radar of the excavator, the current frame point cloud in the laser radar coordinate system is obtained. The laser radar can collect different frames of point cloud data at different times, thereby obtaining each frame of point cloud data of the working environment of the excavator.

[0066] In step 120, as needed, the point cloud may be selectively preprocessed to improve the point cloud data quality and reduce interference data, which is beneficial to improving the quality of three-dimensional map point cloud construction.

[0067] Depending on the work scenario or business needs, you can select one or more of the following methods to preprocess the point cloud. Figure 2 The figure shows a schematic diagram of the preprocessing process of the laser radar point cloud in some embodiments of the present disclosure. It should be noted that: Figure 2 This is only an example, and the order of various preprocessing operations can be set or adjusted as needed.

[0068] The statistical filtering algorithm is used to remove outlier noise points in the current frame point cloud, thereby improving the quality of point cloud data, reducing interference data, and improving the quality of 3D map point cloud construction.

[0069] According to the point cloud reflection intensity information, the dust point cloud and / or dust point cloud in the current frame point cloud are removed. Dust and dust are common scenes in the excavation process, and these noise points will affect the quality of the subsequently constructed three-dimensional map point cloud. There are obvious differences in reflection intensity between dust point clouds and dust point clouds and other environmental point clouds. Therefore, this embodiment filters out such noise points according to the point cloud reflection intensity information. Thereby, the point cloud data quality is improved, the interference data is reduced, and it is beneficial to improve the quality of the three-dimensional map point cloud construction.

[0070] In the acquired point cloud, there are usually point clouds of the excavator device body, such as booms, buckets, etc. These point clouds will affect the quality of the subsequently constructed three-dimensional map point cloud. According to the structural characteristics and motion characteristics of the excavator device, the robot body filtering (robot_body_filter) method in ROS (Robot Operating System) can be used to import the Unified Robot Description Format (URDF) model of the excavator, and remove the excavator body point cloud from the current frame point cloud, thereby filtering out the excavator body point cloud, improving the point cloud data quality, reducing interference data, and helping to improve the quality of three-dimensional map point cloud construction.

[0071] In step 130, the normal distribution information of each voxel of the local map point cloud in the coordinate system of the excavator unloading recorded by the container is obtained so as to directly perform point cloud registration. There is no need to voxelize the local map point cloud and recalculate the normal distribution information of the voxel based on the point cloud points in the voxel every time the point cloud is registered, which reduces time consumption and can quickly and efficiently construct a three-dimensional map point cloud around the excavator.

[0072] The normal distribution information of the voxel in each embodiment of the present disclosure includes, for example: the mean and variance of the normal distribution of the point cloud in the voxel, and the number of points of the point cloud in the voxel, etc.

[0073] The excavator fixed-point excavation, decision-making and planning processes are usually based on the excavator's off-vehicle coordinate system. The transformation matrix from the laser radar coordinate system to the excavator's off-vehicle coordinate system It can be obtained through the current point cloud and local map point cloud registration method. According to this transformation matrix, the point cloud is transformed to the excavator's off-vehicle coordinate system. The whole process can be expressed as:

[0074]

[0075] Among them, M down represents the point cloud in the coordinate system of the excavator, M lidar Represents the point cloud in the radar coordinate system.

[0076] Point cloud registration methods, for example, can use the Iterative Closest Point (ICP) algorithm and the Normal Distribution Transform (NDT) algorithm. The ICP algorithm obtains the pose transformation between the target point cloud and the local map by continuously narrowing the distance between the target point cloud and the local map point cloud. In order to improve the robustness of the ICP algorithm, a common practice is to extract the plane point features and corner point features of the target point cloud, and then obtain the pose transformation by optimizing the distance between the point and the surface, and the point and the line. The earthwork material excavation scene shows more unstructured features, which easily leads to the extraction of plane features and corner point features. It is not limited to specific geometric features. Therefore, in complex unstructured scenes, NDT can have stronger robustness and versatility than the ICP algorithm. Its principle is as follows: Figure 3 shown.

[0077] In step 140 , according to business needs, it can be selectively determined whether the current frame point cloud includes the mining location point cloud.

[0078] Without executing step 140, for example, when the excavator is in a non-excavation state or the point cloud of the excavation point position changes little: for the current frame point cloud, after step 130, the transformation matrix update step 150, the point cloud update step 160, the normal distribution information fusion step 170a, and the global map point cloud determination step 180 can be executed.

[0079] When executing step 140, for example, when the excavator is in an excavation state or the point cloud of the excavation point position changes greatly, for the non-excavation position point cloud, participate in the execution of the transformation matrix update step 150, the point cloud update step 160, the normal distribution information fusion step 170a, and the global map point cloud determination step 180; for the excavation position point cloud, do not participate in the execution of the transformation matrix update step 150 and the normal distribution information fusion step 170a, and participate in the execution of steps 160, 170b, and 180.

[0080] In the excavation state, the three-dimensional map point cloud changes dynamically due to the removal of materials at the excavation location, which can easily affect the subsequent point cloud registration process. Therefore, during the excavation process, the voxel index near the excavation location is obtained based on the excavation location of the bucket and the mechanical structure characteristics of the bucket, and the historical normal distribution values ​​in this part of the voxels are reset to zero and cleared. The normal distribution values ​​of this part of the voxels are recalculated based on the point cloud distribution in this part of the voxels in the current frame, and they do not participate in the subsequent point cloud registration process, thereby making the three-dimensional mapping of the dynamic excavation scene in the material area more robust. The schematic diagram of the voxel update during the excavation process is shown in the figure. Figure 4 shown.

[0081] In step 150, through point cloud registration, that is, according to the residual information of the normal distribution information of each voxel of the local map point cloud obtained by applying the transformation matrix from the laser radar coordinate system to the excavator off-vehicle coordinate system to the current frame point cloud, the transformation matrix is ​​updated to obtain the target transformation matrix.

[0082] Among them, updating the transformation matrix according to the residual information includes: constructing the residual information of the coordinates of the midpoint of the transformed point cloud obtained by applying the transformation matrix to the coordinates of the midpoint of the current frame point cloud relative to the mean of the normal distribution of the same voxel of the local map point cloud; with the goal of reducing the residual information, continuously iteratively updating the transformation matrix to obtain the target transformation matrix.

[0083] Specifically, suppose that there are m point clouds in the voxel with the spatial index value [n1,n2,n3] of the local map point cloud recorded by the container, and the Gaussian distribution they form has a mean of X1 and a variance of σ1. Assume that the coordinates of the transformed point cloud obtained by the transformation matrix acting on the coordinates of the point in the current frame point cloud are [Px,Py,Pz]. Through the coordinate-index transformation k∈[x,y,z],Voxel resolutionTo set the spatial voxel resolution size, convert the point cloud coordinates [Px, Py, Pz] to the spatial index [Ix, Iy, Iz]. After the coordinate-index transformation, assume that there are n point cloud points falling into the voxel with the spatial index value [n1, n2, n3]. The Gaussian distribution of these n point cloud points is the mean X2 and the variance σ2. Then the residual information of the coordinates of the transformed point cloud obtained by the transformation matrix acting on the coordinates of the point in the current frame relative to the mean of the normal distribution of the same voxel in the local map point cloud (that is, the residual information of n point cloud points and the voxel) can be expressed as: Among them, p i represents the i-th point cloud point, e i represents the ith residual, Represents the transformation matrix from the laser radar coordinate system to the excavator's off-vehicle coordinate system. Construct a least squares problem and use the Gauss-Newton algorithm to iterate and reduce the residual to obtain the optimized The Gauss-Newton method for the least squares problem of the NDT algorithm can be expressed as: Where Jac is the residual e pair The Jacobian matrix of The optimization amount to characterize how to optimize In this way, the optimized As the target transformation matrix.

[0084] The initial transformation matrix in the transformation matrix update process is determined according to the excavator rotation angle information. For example, the initial transformation matrix in the transformation matrix update process for the i+1th frame point cloud is for:

[0085]

[0086] Among them, θ i and θ i+1 are the corresponding excavator rotation angles when the i-th frame point cloud and the i+1-th frame point cloud are collected, respectively. z represents the physical offset between the coordinate system of the excavator on the vehicle and the coordinate system of the excavator off the vehicle, which is determined according to the structural characteristics of the excavator. T i Represents the transformation matrix from the i-th frame point cloud to the excavator's off-vehicle coordinate system.

[0087] Generally speaking, the initial transformation matrix is ​​set to the unit matrix or the transformation matrix of the previous frame. This setting method cannot provide a more accurate initial value and is not robust. During the rotation of the excavator, the rotary encoder can output the yaw rotation angle of the upper vehicle relative to the lower vehicle. The output value in a short period of time is relatively accurate. Introducing the rotary encoder information in a short period of time as a priori value can provide a more accurate initial transformation matrix. The initial transformation matrix provided in this embodiment, as the initialization matrix of the subsequent point cloud alignment algorithm, can improve the accuracy of the mapping algorithm and accelerate the convergence speed of the algorithm. The acquisition frame rate of the lidar point cloud is 10HZ, and the rotary encoder information acquisition frequency is 100HZ. The rotary encoder information and the point cloud information are aligned through time alignment.

[0088] In step 160, the fusion target transformation matrix acts on the target transformation point cloud obtained by the current frame point cloud and the local map point cloud to update the local map point cloud.

[0089] Therefore, when the position coordinates of the points in each point cloud are known, the two point clouds are spliced ​​to form a new point cloud based on the position coordinates of the point cloud points.

[0090] In step 170a, for any voxel, the normal distribution information of the target transformed point cloud at the voxel and the normal distribution information of the local map point cloud at the voxel recorded by the container are fused to determine the updated normal distribution information of the local map point cloud at the voxel, and update it to the container.

[0091] The fusion method of the mean of the normal distribution of two point clouds in the same voxel can be expressed as:

[0092]

[0093] Among them, X represents the mean of the normal distribution of the updated local map point cloud in the voxel, X1 represents the mean of the normal distribution of the local map point cloud in the voxel, X2 represents the mean of the normal distribution of the target transformation point cloud in the voxel, m represents the number of points of the local map point cloud in the voxel, and n represents the number of points of the target transformation point cloud in the voxel.

[0094] The fusion method of the variance of the normal distribution of two point clouds in the same voxel can be expressed as:

[0095]

[0096] Among them, X1 represents the mean of the normal distribution of the local map point cloud in the voxel, X2 represents the mean of the normal distribution of the target transformed point cloud in the voxel, m represents the number of points of the local map point cloud in the voxel, n represents the number of points of the target transformed point cloud in the voxel, σ represents the variance of the normal distribution of the updated local map point cloud in the voxel, σ1 represents the variance of the normal distribution of the local map point cloud in the voxel, and σ2 represents the variance of the normal distribution of the target transformed point cloud in the voxel.

[0097] Thus, the normal distribution information of the voxels of the current frame point cloud is fused to the normal distribution information of the voxels of the historical point cloud recorded by the container.

[0098] In step 170b, the normal distribution information of the same voxel in the local map point cloud recorded by the container as that in the excavation location point cloud is cleared and updated to the normal distribution information of the same voxel in the excavation location point cloud.

[0099] That is, the historical normal distribution values ​​in the voxels related to the excavation position are set to zero and cleared, and the normal distribution values ​​of this part of the voxels are recalculated according to the point cloud distribution in this part of the voxels in the current frame, for example, the mean and variance of the normal distribution of the point cloud in the voxel, and the number of points in the point cloud in the voxel.

[0100] Thus, it is possible to promptly respond to the large changes in the three-dimensional map point cloud caused by the removal of materials at the excavation position during the excavation state, and accurately obtain the latest point cloud distribution information near the excavation position.

[0101] In step 180, the local map point cloud and the container records are continuously updated using the current frame point clouds continuously acquired at different times to obtain a three-dimensional global map point cloud.

[0102] Figure 5 FIG. 4 is a schematic diagram showing the construction of a three-dimensional map in this embodiment. Figure 5As shown in the figure, the initial transformation matrix, the current frame point cloud, and the normal distribution information of each voxel of the local map point cloud in the coordinate system of the excavator unloading recorded by the container are input; it is determined whether the current frame point cloud includes the excavation position point cloud; for the non-excavation position point cloud, the transformation matrix is ​​constructed to act on the current frame point cloud to obtain the residual of the normal distribution information of each voxel of the local map point cloud, and the transformation matrix is ​​iteratively updated using the Gauss-Newton algorithm to obtain the optimized target transformation matrix, which is fused with the normal distribution information of the target transformed point cloud at the voxel and the normal distribution information of the local map point cloud recorded by the container at the voxel. The normal distribution information of the updated local map point cloud is determined by the distribution information of the voxel, and the normal distribution information is updated to the container. For the excavation position point cloud, the normal distribution information of the same voxel as the excavation position point cloud in the local map point cloud recorded by the container is cleared and updated to the normal distribution information of the same voxel in the excavation position point cloud. The target transformation point cloud and the local map point cloud obtained by fusing the target transformation matrix to the current frame point cloud are updated. The local map point cloud and the container records are continuously updated by using the current frame point clouds continuously acquired at different times to obtain a three-dimensional global map point cloud.

[0103] This embodiment, on the one hand, integrates the point cloud registration of the rotation coding information, takes the rotation angle information of the excavator as the prior information, and provides a good initial prior pose transformation matrix for the point cloud registration, so as to obtain a more accurate three-dimensional modeling effect of the earthwork materials; on the other hand, a fast point cloud registration method uses the normal distribution point cloud registration algorithm, and for the rotation and excavation process of the excavator, by updating the mean and variance of the normal distribution in different voxels, it avoids the voxelization process of constructing the KD-Tree before each point cloud registration, thereby accelerating the point cloud registration speed.

[0104] Figure 6 A schematic diagram showing the structure of a three-dimensional map construction device in some embodiments of the present disclosure is shown. Figure 6 As shown, the three-dimensional map construction device 600 of this embodiment includes: a memory 610 and a processor 620 coupled to the memory 610, and the processor 620 is configured to execute the three-dimensional map construction method in any of the aforementioned embodiments based on the instructions stored in the memory 610.

[0105] The three-dimensional map construction device 600 may further include an input / output interface 630 , a network interface 640 , a storage interface 650 , etc. These interfaces 630 , 640 , 650 , the memory 610 , and the processor 620 may be connected via a bus 660 , for example.

[0106] The memory 610 may include, for example, a system memory, a fixed non-volatile storage medium, etc. The system memory may store, for example, an operating system, an application program, a boot loader, and other programs.

[0107] The processor 620 may be implemented by a general purpose processor, a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gates or transistors and other discrete hardware components.

[0108] Among them, the input and output interface 630 provides a connection interface for input and output devices such as a display, a mouse, a keyboard, and a touch screen. The network interface 640 provides a connection interface for various networked devices. The storage interface 650 provides a connection interface for external storage devices such as SD cards and USB flash drives. The bus 660 can use any bus structure among a variety of bus structures. For example, the bus structure includes but is not limited to the Industry Standard Architecture (ISA) bus, the Micro Channel Architecture (MCA) bus, and the Peripheral Component Interconnect (PCI) bus.

[0109] Figure 7 A schematic diagram showing the structure of a three-dimensional map construction device in some embodiments of the present disclosure is shown. Figure 7 As shown, the three-dimensional map construction device 700 of this embodiment includes modules 710-760 and at least one of modules 770-790.

[0110] The point cloud acquisition module 710 is configured to acquire the current frame point cloud in the laser radar coordinate system based on the laser radar of the excavator;

[0111] The voxel normal distribution information acquisition module 720 is configured to acquire normal distribution information of each voxel of the local map point cloud in the excavator off-vehicle coordinate system recorded by the container;

[0112] The transformation matrix update module 730 is configured to update the transformation matrix according to the residual information of the normal distribution information of each voxel of the local map point cloud obtained by applying the transformation matrix from the laser radar coordinate system to the excavator off-vehicle coordinate system to the current frame point cloud, so as to obtain a target transformation matrix;

[0113] The point cloud fusion module 740 is configured to fuse the target transformation point cloud obtained by applying the target transformation matrix to the current frame point cloud and the local map point cloud to update the local map point cloud;

[0114] The normal distribution information fusion update module 750 is configured to fuse the normal distribution information of the target transformation point cloud at the voxel and the normal distribution information of the local map point cloud recorded by the container at the voxel for any voxel, so as to determine the updated normal distribution information of the local map point cloud at the voxel, and update it to the container;

[0115] The global map point cloud forming module 760 is configured to continuously update the local map point cloud and the container records using the continuously acquired current frame point clouds to obtain a three-dimensional global map point cloud.

[0116] The excavation position judgment module 770 judges whether the current frame point cloud includes the excavation position point cloud; for the non-excavation position point cloud, it participates in the execution method of modules 710-760; for the excavation position point cloud, it does not participate in the execution of the transformation matrix update module 730 and the normal distribution information fusion module 750, and executes the normal distribution information clearing and re-updating module 780, which is configured to clear the normal distribution information of the same voxel in the local map point cloud recorded by the container as the excavation position point cloud, and update it to the normal distribution information of the same voxel in the excavation position point cloud.

[0117] In some embodiments, the transformation matrix update module 730 is configured to construct the residual information of the coordinates of the transformed point cloud obtained by applying the transformation matrix to the coordinates of the point in the current frame point cloud relative to the mean of the normal distribution of the same voxel of the local map point cloud; with the goal of reducing the residual information, the transformation matrix is ​​continuously iteratively updated to obtain the target transformation matrix. The initial transformation matrix in the transformation matrix update process is determined according to the excavator rotation angle information. For the initial transformation matrix in the transformation matrix update process of the i+1th frame point cloud for:

[0118]

[0119] Among them, θ i and θ i+1 are the corresponding excavator rotation angles when the i-th frame point cloud and the i+1-th frame point cloud are collected, z represents the physical offset between the excavator on-board coordinate system and the excavator off-board coordinate system, T i Represents the transformation matrix from the i-th frame point cloud to the excavator's off-vehicle coordinate system.

[0120] In some embodiments, the normal distribution information fusion update module 750 is configured to calculate and / or

[0121] Among them, X represents the mean of the normal distribution of the updated local map point cloud in the voxel, X1 represents the mean of the normal distribution of the local map point cloud in the voxel, X2 represents the mean of the normal distribution of the target transformed point cloud in the voxel, m represents the number of points of the local map point cloud in the voxel, n represents the number of points of the target transformed point cloud in the voxel, σ represents the variance of the normal distribution of the updated local map point cloud in the voxel, σ1 represents the variance of the normal distribution of the local map point cloud in the voxel, and σ2 represents the variance of the normal distribution of the target transformed point cloud in the voxel.

[0122] In some embodiments, the preprocessing module 790 is configured to perform any one or more of the following: using a statistical filtering algorithm to remove outlier noise points in the current frame point cloud; removing dust point clouds and / or dust point clouds in the current frame point cloud based on point cloud reflection intensity information; removing the excavator body point cloud from the current frame point cloud based on the unified robot description format model of the excavator.

[0123] Figure 8 A schematic diagram showing the structure of a three-dimensional map construction system in some embodiments of the present disclosure is shown. Figure 8 As shown, the three-dimensional map construction system 800 of this embodiment includes: a laser radar 810 installed on an excavator, configured to emit laser signals, collect reflected signals, and obtain point cloud data by calculating and processing the reflected signals; and a three-dimensional map construction device 820, configured to execute the three-dimensional map construction method of each embodiment, please refer to the above for details, which will not be repeated here.

[0124] It should be understood by those skilled in the art that the embodiments of the present disclosure may be provided as methods, systems, or computer program products. Therefore, the present disclosure may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present disclosure may take the form of a computer program product implemented on one or more (non-transient) computer-readable storage media (including but not limited to disk storage, CD-ROM, optical storage, cloud storage, etc.) containing computer program code. A computer program product should be understood as a software product that implements its solution mainly through a computer program.

[0125] The present disclosure is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present disclosure. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0126] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0127] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0128] The above description is only a preferred embodiment of the present disclosure and is not intended to limit the present disclosure. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present disclosure should be included in the protection scope of the present disclosure.

Claims

1. A three-dimensional map construction method, comprising: Based on the laser radar of the excavator, obtain the current frame point cloud in the laser radar coordinate system; Obtain the normal distribution information of each voxel of the local map point cloud in the excavator disembarkation coordinate system recorded by the container; According to the residual information of the normal distribution information of each voxel of the local map point cloud obtained by applying the transformation matrix from the laser radar coordinate system to the excavator off-vehicle coordinate system to the current frame point cloud, the transformation matrix is ​​updated to obtain the target transformation matrix; The target transformation point cloud and the local map point cloud obtained by fusing the target transformation matrix to the current frame point cloud to update the local map point cloud; For any voxel, the normal distribution information of the target transformation point cloud at the voxel and the normal distribution information of the local map point cloud recorded by the container at the voxel are fused to determine the updated normal distribution information of the local map point cloud at the voxel, and update it to the container; By using the continuously acquired point clouds of each current frame, the local map point cloud and the records of the container are continuously updated to obtain a three-dimensional global map point cloud.

2. The three-dimensional map construction method according to claim 1, further comprising: Determine whether the current frame point cloud includes the excavation location point cloud; For the non-excavation location point cloud, participating in executing the steps of claim 1; For the excavation location point cloud, the transformation matrix update step and the normal distribution information fusion step are not executed. The normal distribution information of the same voxel in the local map point cloud recorded by the container as the excavation location point cloud is cleared and updated to the normal distribution information of the same voxel in the excavation location point cloud.

3. The three-dimensional map construction method according to claim 1, wherein: Updating the transformation matrix includes: Construct the residual information of the coordinates of the transformed point cloud obtained by applying the transformation matrix to the coordinates of the point in the current frame relative to the mean of the normal distribution of the same voxel in the local map point cloud; With the goal of reducing the residual information, the transformation matrix is ​​continuously updated iteratively to obtain the target transformation matrix.

4. The three-dimensional map construction method according to claim 1, wherein: The initial transformation matrix in the transformation matrix updating process is determined according to the excavator rotation angle information.

5. The three-dimensional map construction method according to claim 4, wherein: The initial transformation matrix in the transformation matrix update process of the i+1th frame point cloud for: Among them, θ i and θ i+1 are the corresponding excavator rotation angles when the i-th frame point cloud and the i+1-th frame point cloud are collected, z represents the physical offset between the excavator on-board coordinate system and the excavator off-board coordinate system, T i Represents the transformation matrix from the i-th frame point cloud to the excavator's off-vehicle coordinate system.

6. The three-dimensional map construction method according to any one of claims 1 to 5, wherein: For any voxel, the normal distribution information of the target transformation point cloud at the voxel and the normal distribution information of the local map point cloud recorded by the container at the voxel are fused to determine the updated normal distribution information of the local map point cloud at the voxel, including: calculate Among them, X represents the mean of the normal distribution of the updated local map point cloud in the voxel, X1 represents the mean of the normal distribution of the local map point cloud in the voxel, X2 represents the mean of the normal distribution of the target transformation point cloud in the voxel, m represents the number of points of the local map point cloud in the voxel, and n represents the number of points of the target transformation point cloud in the voxel.

7. The three-dimensional map construction method according to any one of claims 1 to 5, wherein: For any voxel, the normal distribution information of the target transformation point cloud at the voxel and the normal distribution information of the local map point cloud recorded by the container at the voxel are fused to determine the updated normal distribution information of the local map point cloud at the voxel, including: calculate Among them, X1 represents the mean of the normal distribution of the local map point cloud in the voxel, X2 represents the mean of the normal distribution of the target transformed point cloud in the voxel, m represents the number of points of the local map point cloud in the voxel, n represents the number of points of the target transformed point cloud in the voxel, σ represents the variance of the normal distribution of the updated local map point cloud in the voxel, σ1 represents the variance of the normal distribution of the local map point cloud in the voxel, and σ2 represents the variance of the normal distribution of the target transformed point cloud in the voxel.

8. The three-dimensional map construction method according to any one of claims 1 to 5, further comprising: Use statistical filtering algorithm to remove outlier noise in the point cloud of the current frame.

9. The three-dimensional map construction method according to any one of claims 1 to 5, further comprising: According to the point cloud reflection intensity information, the dust point cloud and / or the dust point cloud in the current frame point cloud are removed.

10. The three-dimensional map construction method according to any one of claims 1 to 5, further comprising: Based on the unified robot description format model of the excavator, the excavator body point cloud is removed from the current frame point cloud.

11. The three-dimensional map construction method according to any one of claims 1 to 5, wherein: The normal distribution information of a voxel includes: the mean and variance of the normal distribution of the point cloud at the voxel, and the number of points of the point cloud at the voxel.

12. A three-dimensional map construction device, comprising: A module for executing the three-dimensional map construction method according to any one of claims 1 to 11.

13. A three-dimensional map construction device, comprising: Memory; And a processor coupled to the memory, characterized in that the processor is configured to execute the three-dimensional map construction method according to any one of claims 1-11 based on instructions stored in the memory.

14. A three-dimensional map construction system, comprising: The laser radar installed on the excavator is configured to emit laser signals, collect reflected signals, and obtain point cloud data by calculating and processing the reflected signals; as well as A three-dimensional map construction device is configured to execute the three-dimensional map construction method according to any one of claims 1-11.

15. A computer-readable storage medium having computer instructions stored thereon, characterized in that: When the computer instructions are executed by a processor, the steps of the three-dimensional map construction method described in any one of claims 1-11 are implemented.

16. A computer program product comprising computer instructions, characterized in that When the computer instructions are executed by a processor, the steps of the three-dimensional map construction method described in any one of claims 1-11 are implemented.

Images