Method and system for constructing gradient influence prevention 2D map through 3D laser point cloud

Abstract

The invention belongs to the technical field of automatic driving and robot navigation, and particularly relates to a method and system for constructing a gradient influence prevention 2D map through 3D laser point clouds, and the method comprises the steps: obtaining point cloud data; processing the point cloud data; performing ground segmentation according to the processed point cloud data; height compensation is carried out after ground segmentation; constructing a 2D map after height compensation; the influence of road gradient and unevenness on map construction is effectively avoided through ground segmentation and height compensation in the 2D map construction process, and the accuracy and reliability of the map are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving and robot navigation, and in particular relates to a method and system for constructing a 2D map with slope protection through 3D laser point cloud. Background Art

[0002] With the development of autonomous driving technology and robot navigation, high-precision maps have become the basis for safe and reliable navigation. Current map construction methods are usually based on 3D lidar point cloud data. The information in the three-dimensional space is projected onto a two-dimensional plane through a point cloud processing algorithm to form a map that can be used for path planning. This method performs well on flat terrain, but in actual applications, because the road may have a certain slope or unevenness, the constructed 2D map cannot accurately reflect the actual terrain, thus affecting the accuracy of path planning.

[0003] In the field of autonomous driving, high-precision maps are not only the basis for vehicle positioning and navigation, but also the key to ensuring driving safety. Although the traditional 2D map construction method can provide better navigation support on relatively flat ground, when encountering steep sections of road or uneven ground, the map is often distorted due to the loss of height information. This distortion not only affects the positioning accuracy of the vehicle, but also causes deviations in path planning and even causes safety hazards. For example, in mountainous or hilly areas, vehicles need to accurately identify changes in the slope of the road while driving, otherwise it may lead to misjudgment and affect driving safety.

[0004] Similarly, in the field of robot navigation, high-precision maps are also crucial. In the fields of industrial automation, logistics and distribution, robots need to navigate autonomously in complex environments, which requires maps to accurately reflect terrain features. However, existing point cloud processing algorithms often ignore the impact of height information when dealing with terrain with large slope changes, resulting in maps that do not match the actual terrain, affecting the robot's path planning and obstacle avoidance capabilities. For example, inside a warehouse or factory, the ground may have varying degrees of inclination or undulation. If the 2D map fails to accurately represent these terrain features, the robot's navigation performance will be greatly reduced.

[0005] Especially in complex terrain conditions, such as construction sites, mines or forests, the slope and unevenness of the terrain are more significant. In these environments, traditional 2D map construction methods often cannot provide reliable navigation support. For example, on a construction site, construction vehicles need to be accurately positioned and routed according to the characteristics of the terrain; in mines, transport vehicles also need to avoid dangerous areas. If the 2D map cannot accurately reflect these terrain features, it will seriously affect work efficiency and safety.

[0006] Therefore, due to the technical problem that the terrain features cannot be accurately reflected in the 2D map construction of the ancient city, it is necessary to design a new method and system for 3D laser point cloud construction to prevent slope influence on 2D map.

[0007] It should be noted that the above information disclosed in this background technology section is only used to understand the background technology of the present application concept, and therefore, the above description is not considered to constitute information of the prior art. Summary of the invention

[0008] The embodiments of the present disclosure at least provide a method and system for constructing a 2D map that is protected from slope impact using 3D laser point clouds.

[0009] In a first aspect, an embodiment of the present disclosure provides a method for constructing a 2D map to prevent slope impact using a 3D laser point cloud, comprising:

[0010] Get point cloud data;

[0011] Processing point cloud data;

[0012] Perform ground segmentation based on the processed point cloud data;

[0013] Perform height compensation after ground segmentation;

[0014] Construct a 2D map after altitude compensation.

[0015] In an optional implementation, the method for acquiring point cloud data includes:

[0016] Use 3D laser radar to obtain point cloud data in the environment.

[0017] In an optional implementation, the method for processing point cloud data includes:

[0018] For each point cloud point, calculate its local neighborhood features;

[0019] The local neighborhood features include: height and curvature;

[0020] Field calculation:

[0021]

[0022] Where Di represents the reference point P i With other points P j The distance between i ,y i , z i ) is the i-th reference point P i The coordinates of (x j ,y j , z j ) is the jth point P jThe coordinates of the selected distance are less than the radius R p The point is taken as the field, when Di≤R p When j Denote the reference point P i According to the density and scene of the point cloud, the radius R p The general value is between 0.1m and 0.3m;

[0023] Height mean: Calculate the domain point P j The mean of the coordinates, z j is the height of the neighborhood point, and n is the number of neighborhood points:

[0024]

[0025] Height variance calculation: Calculate the variance of the coordinates of the field points:

[0026]

[0027] Curvature calculation:

[0028] The covariance matrix C, where μ x is the mean vector of the domain points, according to the reference point P i The coordinates (x i ,y i , z i ), and get the coordinate vector p i For [x i y i z i ] T , (p j -μ x ) is the coordinate vector of the jth neighborhood point minus the mean vector, (p j -μ x ) T Represents transpose:

[0029]

[0030] Eigenvalue: Calculate the eigenvalues ​​λ1,λ2,λ3 of the covariance matrix;

[0031] Curvature is the ratio of the maximum eigenvalue to the minimum eigenvalue:

[0032]

[0033] In an optional implementation, the method for performing ground segmentation according to the processed point cloud data includes:

[0034] According to the calculated local neighborhood features, the ground points are distinguished from other non-ground points, where ∈ hIndicates the height threshold, which is generally between 0.05m and 0.1m, ∈ K is the curvature threshold, usually set between 0.05 and 0.1, z j Indicates the height of the jth point on the z-axis, μ z It is the height average in this field;

[0035]

[0036] In an optional implementation, the method for performing height compensation after ground segmentation includes:

[0037] For the ground points obtained by segmentation, the height compensation value is calculated according to its local neighborhood characteristics to eliminate the error caused by the road slope and / or unevenness;

[0038] Calculate the new height of the neighborhood points. After removing the ground points, the remaining points are recorded as P′ i (x′ i , y′ i , z′ i ) Calculate the new height mean μ′ of the domain z :

[0039]

[0040] Calculation of the new curvature:

[0041] Similarly, the covariance matrix C′ is calculated, where μ′ x is the mean vector of the domain points, P′ j is the coordinate of the jth new domain point, if its coordinate is (x′ j , y′ j , z′ j ), then p′ j Its coordinate vector, denoted as [x′ j y′ j z′ j ] T , (p′ j -μ′ x ) is the coordinate vector of the jth neighborhood point minus the mean vector, (p′ j -μ′ x ) T Represents transpose:

[0042]

[0043] The new eigenvalues ​​λ′1,λ′2,λ′3 of the covariance matrix C′ are calculated, and its eigenvectors are denoted as v1,v2,v3. The plane normal vector n is selected as the eigenvector min(v1,v2,v3) corresponding to the minimum eigenvalue, denoted as n=[a bc] T

[0044] Using the reference point P′ i (x′ i , y′ i , z′ i ) Let the plane reference equation be: ax+by+cz+d=0

[0045] Calculation results in: d = -(ax′ i +by′ i +cz′ i ),

[0046] Compensation calculation: For each new neighborhood point P′ j (x′ j , y′ j , z′ j ) Calculate the vertical distance d from the fitting plane j That is the compensation amount on the z-axis:

[0047]

[0048] For each remaining point P′ i (x′ i , y′ i , z′ i )

[0049] Adjusted height z new :

[0050] z new = z′ i -d j ;

[0051] The new coordinates of the processed point cloud are marked as Po i (x′ i , y′ i , z new ).

[0052] In an optional implementation, the method for constructing a 2D map after height compensation includes:

[0053] Set the first GPS point received as the coordinate origin, construct the global coordinate system, and obtain the robot position as (x0, y0, z0) for translation;

[0054] Get the quaternion q(x, y, z, w) from the sensor;

[0055] The quaternion q is expressed as q = (w, x, y, z) where w is the real part and (x, y, z) is the imaginary part;

[0056] The rotation matrix R is a 3x3 matrix:

[0057]

[0058] The matrix elements are:

[0059] m 00 =1-2(y 2 +z 2 );

[0060] m 01 =2(xy-wz);

[0061] m 02 =2(xz+wy);

[0062] m 10 =2(xy+wz);

[0063] m 11 =1-2(x 2 +z 2 );

[0064] m 12 =2(yz-wx);

[0065] m 20 =2(xz-wy);

[0066] m 21 =2(yz+wx);

[0067] m 22 =1-2(x 2 +y 2 );

[0068] A point Po in the point cloud i =(x′ i , y′ i , z new ), convert it to the new coordinate system, first construct the homogeneous coordinate form Po i ′, then apply the rotation matrix R and the translation vector T = (x0, y0, z0) obtained from the robot's position to get the new point Po i ″:

[0069] Po i ′=[x′ i y′ i z′ new 1] T ;

[0070] Po i ″=R·Po i ′+T;

[0071] Get a point of the converted point cloud and record it as Poi ″=(x,y,z), define map parameters, where the value of Minh changes dynamically according to the position of the point, the farther the distance, the larger the value, which is used to eliminate the impact of uneven roads on map construction: Gw: grid width; Gh: grid height; Re: grid resolution, the smaller the value, the greater the map accuracy; Minh: minimum detection height; Maxh: maximum detection height; Ro: obstacle expansion value;

[0072] Enter the size of the global map, such as width w = 40m, height h = 40m, define the grid resolution size as Re = 0.1, which means that one grid represents a width and length of 0.1m, and the map grid size is 400*400. The actual map size is 40m*40m:

[0073] Gw=w / Re=400;

[0074] Gh=h / Re=400;

[0075] Filter the points in the point cloud Minh<z<Maxh, remove the unsegmented ground points again, and preliminarily estimate the position of the point cloud on the grid map. Define the center position of the grid map as the robot's starting position. The position of position (0, 0) on the grid map is (Gw / 2, Gh / 2) (200, 200). For example, the value of the point (x, y, z) is (2.1, 3.1, 4.1):

[0076]

[0077] The position of the point (x′, y′) on the grid map is (221, 331), so the grid at row 221 and column 331 is marked to indicate an obstacle.

[0078] Map obstacle layer calibration, Ro is recorded as the obstacle expansion value, which is generally between 2 and 3, indicating that the obstacle contour is expanded between 0.2 meters and 0.3 meters. The positions of all grid points smaller than the radius Ro are obtained in a loop and put into the obstacle point set obs, which is recorded as Represents the total number of obstacle points in the set:

[0079]

[0080] Take the point (221, 331) obtained above as an obstacle, take Ro=2, and perform obstacle expansion to obtain other obstacle points (222, 331), (223, 331), (221, 332), (221, 333), (220, 331), (219, 331), (221, 330), (221, 329), (222, 332), (222, 330), (220, 332), (220, 330) and then put all the points into the set obs;

[0081] Reset all data in the map grid to -1, indicating an unknown state:

[0082] Gd = [-1] * (Gw * Gh);

[0083] After obtaining the position from obs, mark the position of the point as 100 and record it as an obstacle to obtain the final map.

[0084] In a second aspect, the embodiment of the present disclosure further provides a method for constructing a 2D map to prevent slope impact using a 3D laser point cloud, comprising:

[0085] An acquisition module, configured to acquire point cloud data;

[0086] A processing module configured to process the point cloud data;

[0087] a segmentation module configured to perform ground segmentation based on the processed point cloud data;

[0088] A compensation module, configured to perform height compensation after ground segmentation;

[0089] A map module is configured to construct a 2D map after altitude compensation.

[0090] In a third aspect, an embodiment of the present disclosure further provides a computer device including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the method for constructing a slope-proof 2D map using a 3D laser point cloud.

[0091] In a fourth aspect, the embodiments of the present disclosure further provide a computer-readable storage medium having a computer program / instruction stored thereon, which, when executed by a processor, implements the steps of the method for constructing a slope-resistant 2D map using 3D laser point clouds.

[0092] In a fifth aspect, the embodiments of the present disclosure further provide a computer program product, including a computer program / instruction, which, when executed by a processor, implements the steps of the method for constructing a slope-resistant 2D map using a 3D laser point cloud.

[0093] The beneficial effect of the present invention is that the method for constructing a 2D map to prevent slope influence by the 3D laser point cloud comprises: acquiring point cloud data; processing the point cloud data; performing ground segmentation according to the processed point cloud data; performing height compensation after ground segmentation; constructing a 2D map after height compensation; and achieving effective avoidance of the influence of road slope and unevenness on map construction through ground segmentation and height compensation in the process of constructing a 2D map, thereby improving the accuracy and reliability of the map.

[0094] Other features and advantages of the present invention will be described in the following description, and partly become apparent from the description, or understood by practicing the present invention. The purpose and other advantages of the present invention are realized and obtained by the structures particularly pointed out in the description, claims and drawings.

[0095] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, this article specifically cites preferred embodiments and provides detailed descriptions as follows in conjunction with the attached drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0097] Figure 1 A flowchart of a method for constructing a 2D map to prevent slope impact from 3D laser point cloud provided in an embodiment of the present disclosure;

[0098] Figure 2 A schematic diagram of a simulated three-dimensional and two-dimensional point cloud fusion map provided in an embodiment of the present disclosure;

[0099] Figure 3 A schematic diagram of a simulated two-dimensional map provided in an embodiment of the present disclosure;

[0100] Figure 4 A schematic diagram of a simulated two-dimensional map saving result provided by an embodiment of the present disclosure;

[0101] Figure 5 An actual unprocessed 3D point cloud map provided by an embodiment of the present disclosure;

[0102] Figure 6 A 2D map after actual segmentation of ground points provided in an embodiment of the present disclosure;

[0103] Figure 7 A 2D and 3D fused map after actual segmentation of ground points is provided in an embodiment of the present disclosure. DETAILED DESCRIPTION

[0104] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0105] In this article, when it is mentioned that the first component is located on the second component, this may mean that the first component may be directly formed on the second component, or the third component may be interposed between the first component and the second component. In addition, in the drawings, in order to effectively describe the technical content, the thickness of the components may be exaggerated or reduced.

[0106] As used herein, the phrases "in one embodiment," "according to one embodiment," "in some embodiments," and the like generally refer to the fact that the particular feature, structure, or characteristic following the phrase may be included in at least one embodiment of the present disclosure. Therefore, a particular feature, structure, or characteristic may be included in more than one embodiment of the present disclosure, so that these phrases do not necessarily refer to the same embodiment. As used herein, the terms "example," "exemplary," and the like are used to "serve as an example, instance, or illustration." Any implementation, aspect, or design described herein as "example" or "exemplary" is not necessarily to be construed as preferred or superior to other implementations, aspects, or designs. On the contrary, the use of the terms "example," "exemplary," and the like is intended to present concepts in a concrete manner.

[0107] Some embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. In the absence of conflict, the following embodiments and features in the embodiments can be combined with each other.

[0108] like Figures 1 to 7 As shown, at least one disclosed embodiment provides a method for constructing a 2D map with 3D laser point cloud to prevent slope influence, including: acquiring point cloud data; processing point cloud data; performing ground segmentation according to the processed point cloud data; performing height compensation after ground segmentation; constructing a 2D map after height compensation; achieving effective avoidance of the influence of road slope and unevenness on map construction through ground segmentation and height compensation in the process of constructing 2D map, thereby improving the accuracy and reliability of the map. It can effectively process 3D laser point cloud data, and fully consider the slope and unevenness of the terrain when constructing 2D map to ensure the accuracy and reliability of the map. 3D point cloud data is processed by advanced algorithms, key height information is extracted, and integrated into the 2D map, so that the 2D map is closer to the actual terrain and the accuracy and reliability of path planning are improved.

[0109] In an optional embodiment, the method for obtaining point cloud data includes: obtaining point cloud data in the environment through a 3D laser radar; the laser radar adopts Unitree 4D LiDAR L1 of Yushu Technology to receive point cloud data from the 3D laser radar.

[0110] In an optional implementation, the method for processing point cloud data includes: for each point cloud point, calculating its local neighborhood features; the local neighborhood features include: height and curvature; domain calculation:

[0111]

[0112] Where Di represents the reference point P i With other points P j The distance between i ,y i , z i ) is the i-th reference point P i The coordinates of (x j ,y j , z j ) is the jth point P j The coordinates of the selected distance are less than the radius R p The point is taken as the field, when Di≤R p When j Denote the reference point P i According to the density and scene of the point cloud, the radius R p The general value is between 0.1m and 0.3m;

[0113] Height mean: Calculate the domain point P j The mean of the coordinates, z j is the height of the neighborhood point, and n is the number of neighborhood points:

[0114]

[0115] Height variance calculation: Calculate the variance of the z coordinates of the field points:

[0116]

[0117] Curvature calculation: covariance matrix C, where μ x is the mean vector of the domain points, according to the reference point P i The coordinates (x i ,y i , z i ), and get the coordinate vector p i For [x i y i z i ]T , (p j -μ x ) is the coordinate vector of the jth neighborhood point minus the mean vector, (p j -μ x ) T Represents transpose:

[0118]

[0119] Eigenvalue: Calculate the eigenvalues ​​λ1,λ2,λ3 of the covariance matrix; Cur curvature is the ratio of the maximum eigenvalue to the minimum eigenvalue:

[0120]

[0121] In an optional embodiment, the method for ground segmentation based on the processed point cloud data includes: distinguishing ground points from other non-ground points based on the calculated local neighborhood features; distinguishing ground points from other non-ground points based on the calculated local neighborhood features, where ∈ h Indicates the height threshold, which is generally between 0.05m and 0.1m, ∈ K is the curvature threshold, usually set between 0.05 and 0.1, z j Indicates the height of the jth point on the z-axis, μ z It is the height average in this field;

[0122]

[0123] A plane fitting algorithm is used to find the best fitting plane through multiple iterations, and points close to the plane are regarded as ground points.

[0124] In an optional embodiment, the method for performing height compensation after ground segmentation includes: for the ground points obtained by segmentation, calculating the height compensation value according to the local neighborhood characteristics to eliminate the error caused by the road slope and / or unevenness; calculating the new height of the neighborhood points, after filtering out the ground points, the remaining points are recorded as P′ i (x′ i , y′ i , z′ i ) Calculate the new height mean μ′ of the domain z :

[0125]

[0126] Calculation of the new curvature, that is, the curvature after screening:

[0127] Similarly, the covariance matrix C′ is calculated, where μ′ x is the mean vector of the domain points, P′ jis the coordinate of the jth new domain point, if its coordinate is (x′ j , y′ j , z′ j ), then p′ j Its coordinate vector, denoted as [x′ j y′ j z′ j ] T , (p′ j -μ′ x ) is the coordinate vector of the jth neighborhood point minus the mean vector, (p′ j -μ′ x ) T Represents transpose:

[0128]

[0129] The new eigenvalues ​​λ′1,λ′2,λ′3 of the covariance matrix C′ are calculated, and its eigenvectors are denoted as v1,v2,v3. The plane normal vector n is selected as the eigenvector min(v1,v2,v3) corresponding to the minimum eigenvalue, denoted as n=[a bc] T ;

[0130] Using the reference point P′ i (x′ i , y′ i , z′ i ) Let the plane reference equation be: ax+by+cz+d=0;

[0131] Calculation results in: d = -(ax′ i +by′ i +cz′ i );

[0132] Compensation calculation: For each new neighborhood point P′ j (x′ j , y′ j , z′ j ) Calculate the vertical distance d from the fitting plane j That is the compensation amount on the z-axis:

[0133]

[0134] For each remaining point P′ i (x′ i , y′ i , z′ i ), the adjusted height z new :

[0135] z new = z′ i -dj ;

[0136] Get the new coordinates Po of the processed point cloud i (x′ i , y′ i , z new ).

[0137] In an optional implementation, the method for constructing a 2D map after height compensation includes: using a single-antenna Tiangong 980 as a fixed base station, transmitting differential information to the cloud server mqtt through the Hanfeng DTU module, and using a Tiangong 982 dual-antenna mobile station to obtain differential information from mqtt through WIFI and publish precise positioning information. Set the first value received as the coordinate origin, construct a global coordinate system, and obtain the robot position x0, y0, z0 for translation; imu is WT901C485 of Witt Intelligent, and obtain the quaternion q (x, y, z, w) from the sensor imu;

[0138] The quaternion q is represented by q = (w, x, y, z) where w is the real part and (x, y, z) is the imaginary part; the rotation matrix R is a 3x3 matrix:

[0139]

[0140] The matrix elements are:

[0141] m 00 =1-2(y 2 +z 2 );

[0142] m 01 =2(xy-wz);

[0143] m 02 =2(xz+wy);

[0144] m 10 =2(xy+wz);

[0145] m 11 =1-2(x 2 +z 2 );

[0146] m 12 =2(yz-wx);

[0147] m 20 =2(xz-wy);

[0148] m 21 =2(yz+wx);

[0149] m 22 =1-2(x2 +y 2 );

[0150] A point Po in the point cloud i =(x′ i , y′ i , z new ), convert it to the new coordinate system, first construct the homogeneous coordinate form Po i ′, then apply the rotation matrix R and the translation vector T = (x0, y0, z0) obtained from the robot's position to obtain the new point Po i ″:

[0151] Po i ′=[x′ i y′ i z′ new 1] T ;

[0152] Po i ″=R·Po i ′+T;

[0153] Get a point Po of the converted point cloud i ″=(x,y,z), define map parameters, where the value of Minh changes dynamically according to the position of the point, the farther the distance, the larger the value, which is used to eliminate the impact of uneven roads on map construction: Gw: grid width (unit: pixel); Gh: grid height (unit: pixel); Re: grid resolution (unit: meter / pixel); Minh: minimum detection height (unit: meter); Maxh: maximum detection height (unit: meter); Ro: obstacle expansion value; input the size of the global map, include building a 40m x 40m map eg: w=40m, h=40m, the resolution is defined as Re=0.1, indicating that a grid represents a width and length of 0.1m, and the map grid size is 400*400, and the actual map size is 40m*40m:

[0154] Gw=w / Re=400;

[0155] Gh=h / Re=400;

[0156] Filter the points in the point cloud Minh<z<Maxh, remove the ground points that may not be segmented again, and preliminarily estimate the position of the point cloud on the map. Define the center position of the grid map as the starting position of the robot. The position of position (0, 0) on the grid map is (Gw / 2, Gh / 2) is (200, 200). For example, the value of the point (x, y, z) is (2.1, 3.1, 4.1):

[0157] x′=x / Re+Gw / 2=221

[0158] y′=y / Re+Gh / 2=331;

[0159] The position of the point (x′, y′) on the grid map is (221, 331), so the grid at row 221 and column 331 is marked to indicate an obstacle.

[0160] Map obstacle layer calibration, Ro is recorded as the obstacle expansion value, which is generally between 2 and 3, indicating that the obstacle contour is expanded between 0.2 meters and 0.3 meters. The positions of all grid points smaller than the radius Ro are obtained in a loop and put into the obstacle point set obs, which is recorded as Represents the total number of obstacle points in the set:

[0161]

[0162] Take the point (221, 331) obtained above as an obstacle, take Ro=2, and perform obstacle expansion to obtain other obstacle points (222, 331), (223, 331), (221, 332), (221, 333), (220, 331), (219, 331), (221, 330), (221, 329), (222, 332), (222, 330), (220, 332), (220, 330) and then put all the points into the set obs;

[0163] Reset all data in the map grid to -1, indicating an unknown state:

[0164] Gd = [-1] * (Gw * Gh);

[0165] After obtaining the position from obs, mark the position of Gd as 100 and record it as an obstacle to obtain the final map.

[0166] At least one other disclosed embodiment also provides a method for constructing a 2D map that is resistant to slope impact using a 3D laser point cloud, comprising: an acquisition module configured to acquire point cloud data; a processing module configured to process the point cloud data; a segmentation module configured to perform ground segmentation based on the processed point cloud data; a compensation module configured to perform height compensation after ground segmentation; and a map module configured to construct a 2D map after height compensation.

[0167] In at least one other disclosed embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-mentioned method of constructing a slope-proof 2D map using 3D laser point clouds.

[0168] At least one other disclosed embodiment further provides a computer-readable storage medium having a computer program / instruction stored thereon, which, when executed by a processor, implements the steps of the above-mentioned method for constructing a slope-resistant 2D map using 3D laser point clouds.

[0169] In at least one other disclosed embodiment, a computer program product is also provided, including a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned method for constructing a slope-proof 2D map using a 3D laser point cloud.

[0170] In summary, the method for constructing a 2D map to prevent slope influence by using 3D laser point cloud includes: acquiring point cloud data; processing the point cloud data; performing ground segmentation according to the processed point cloud data; performing height compensation after ground segmentation; and constructing a 2D map after height compensation. In the process of constructing a 2D map, ground segmentation and height compensation are used to effectively avoid the influence of road slope and unevenness on map construction, thereby improving the accuracy and reliability of the map.

[0171] Based on the above ideal embodiments of the present invention, the relevant staff can make various changes and modifications without departing from the technical concept of the present invention through the above description. The technical scope of the present invention is not limited to the contents of the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A method for constructing a 2D map to prevent slope impact using 3D laser point cloud, characterized in that: include: Get point cloud data; Processing point cloud data; Perform ground segmentation based on the processed point cloud data; Perform height compensation after ground segmentation; Construct a 2D map after altitude compensation.

2. The method for constructing a 2D map to prevent slope impact using 3D laser point cloud as claimed in claim 1, characterized in that: The method for obtaining point cloud data comprises: Use 3D laser radar to obtain point cloud data in the environment.

3. The method for constructing a 2D map for preventing slope impact using 3D laser point cloud according to claim 2, characterized in that: The method for processing point cloud data comprises: For each point cloud point, calculate its local neighborhood features; The local neighborhood features include: height and curvature; Field calculation: Where Di represents the reference point P i With other points P j The distance between i ,y i , z i ) is the i-th reference point P i The coordinates of (x j ,y j , z j ) is the jth point P j The coordinates of the selected distance are less than the radius R p The point is taken as the field, when Di≤R p When j Denote the reference point P i Field points; Height mean: Calculate the domain point P j The mean of the coordinates, z j is the height of the neighborhood point, and n is the number of neighborhood points: Height variance calculation: Calculate the variance of the coordinates of the field points: Curvature calculation: The covariance matrix C, where μ x is the mean vector of the domain points, according to the reference point P i The coordinates (x i ,y i , z i ), and get the coordinate vector p i For [x i y i z i ] T , (p j -μ x ) is the coordinate vector of the jth neighborhood point minus the mean vector, (p j -μ x ) T Represents transpose: Eigenvalue: Calculate the eigenvalues ​​λ1,λ2,λ3 of the covariance matrix; Curvature is the ratio of the maximum eigenvalue to the minimum eigenvalue:

4. The method for constructing a 2D map for preventing slope impact using 3D laser point cloud according to claim 3, characterized in that: The method for performing ground segmentation according to the processed point cloud data comprises: According to the calculated local neighborhood features, the ground points are distinguished from other non-ground points, where ∈ h represents the height threshold, ∈ K is the curvature threshold, z j represents the height of the jth point on the z-axis, μ z It is the height average in this field; 5. The method for constructing a 2D map for preventing slope impact using 3D laser point cloud as claimed in claim 4, characterized in that: The method for performing height compensation after ground segmentation comprises: For the ground points obtained by segmentation, the height compensation value is calculated according to its local neighborhood characteristics to eliminate the error caused by the road slope and / or unevenness; Calculate the new height of the neighborhood points. After removing the ground points, the remaining points are recorded as P′ i (x′ i , y′ i , z′ i ) Calculate the new height mean μ′ of the domain z : Calculation of the new curvature: Calculate the covariance matrix C′, where μ′ x is the mean vector of the domain points, P′ j is the coordinate of the jth new domain point, if its coordinate is (x′ j , y′ j , z′ j ), then p′ j Its coordinate vector, denoted as [x′ j y′ j z′ j ] T , (p′ j -μ′ x ) is the coordinate vector of the jth neighborhood point minus the mean vector, (p′ j -μ′ x ) T Represents transpose: The new eigenvalues ​​λ′1,λ′2,λ′3 of the covariance matrix C′ are calculated, and its eigenvectors are denoted as v1,v2,v3. The plane normal vector n is selected as the eigenvector min(v1,v2,v3) corresponding to the minimum eigenvalue, denoted as n=[a bc] T ; Using the reference point P′ i (x′ i , y′ i , z′ i ) Let the plane reference equation be: ax+by+cz+d=0; Calculation results in: d = -(ax′ i +by′ i +cz′ i ); Compensation calculation: For each new neighborhood point P′ j (x′ j , y′ j , z′ j ) Calculate the vertical distance d from the fitting plane j That is the compensation amount on the z-axis: For each remaining point P′ i (x′ i , y′ i , z′ i ), the adjusted height z new : z new =z′ i -d j ; The new coordinates of the processed point cloud are marked as Po i (x′ i , y′ i , z new ).

6. The method for constructing a 2D map for preventing slope impact using 3D laser point cloud according to claim 5, characterized in that: The method for constructing a 2D map after height compensation comprises: Set the first GPS point received as the coordinate origin, construct the global coordinate system, and obtain the robot position as (x0, y0, z0) for translation; Get the quaternion q(x, y, z, w) from the sensor; The quaternion q is expressed as q = (w, x, y, z) where w is the real part and (x, y, z) is the imaginary part; The rotation matrix R is a 3x3 matrix: The matrix elements are: m 00 =1-2(y 2 +z 2 ); m 01 =2(xy-wz); m 02 =2(xz+wy); m 10 =2(xy+wz); m 11 =1-2(x 2 +z 2 ); m 12 =2(yz-wx); m 20 =2(xz-wy); m 21 =2(yz+wx); m 22 =1-2(x 2 +y 2 ); A point Po in the point cloud i =(x′ i , y′ i , z new ), convert it to the new coordinate system, first construct the homogeneous coordinate form Po i ′, then apply the rotation matrix R and the translation vector T = (x0, y0, z0) obtained from the robot's position to obtain the new point Po i ″: Po i ′=[x′i y′i z′ new 1] T ; Po i ″=R·Po i ′+T; Get a point of the converted point cloud and record it as Po i ″=(x,y,z), define the map parameters, where the value of Minh changes dynamically according to the position of the point: Gw: grid width; Gh: grid height; Re: grid resolution, the smaller the value, the greater the map accuracy; Minh: minimum detection height; Maxh: maximum detection height; Ro: obstacle expansion value; Filter the points in the point cloud Minh<z<Maxh, remove the ground points that have not been segmented again, and preliminarily estimate the position of the point cloud on the grid map. Define the center position of the grid map as the starting position of the robot. The position (0, 0) on the grid map is (Gw / 2, Gh / 2). x′=x / Re+Gw / 2 y′=y / Re+Gh / 2 Get the position of this point (x′, y′) on the grid map. This grid indicates an obstacle. The map obstacle layer is calibrated, Ro is recorded as the obstacle expansion value, and the positions of all grid points smaller than the radius Ro are obtained in a loop and put into the obstacle point set obs, recorded as Represents the total number of obstacle points in the set: The above-obtained points are regarded as obstacles. Obstacle expansion is performed according to Ro to obtain other obstacle points, and then all the points are put into the set obs. Reset all data in the map grid to -1, indicating an unknown state: Gd = [-1] * (Gw * Gh); After obtaining the position from obs, mark the position of the point as 100 and record it as an obstacle to obtain the final map.

7. A method for constructing a 2D map to prevent slope impact using 3D laser point cloud, characterized in that: include: An acquisition module, configured to acquire point cloud data; A processing module configured to process the point cloud data; a segmentation module, configured to perform ground segmentation based on the processed point cloud data; A compensation module, configured to perform height compensation after ground segmentation; A map module is configured to construct a 2D map after altitude compensation.

8. A computer device comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method for constructing a slope-proof 2D map using 3D laser point cloud as described in claim 1.

9. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method for constructing a slope-proof 2D map using 3D laser point clouds as described in claim 1 are implemented.

10. A computer program product comprising a computer program / instructions, characterized in that When the computer program / instructions are executed by a processor, the steps of the method for constructing a slope-proof 2D map using 3D laser point clouds as described in claim 1 are implemented.

Images