Two-dimensional map terrain marking method based on multi-frame point cloud fusion data

Abstract

The invention relates to the technical field of positioning navigation and map construction, in particular to a two-dimensional map terrain marking method based on multi-frame point cloud fusion data. According to the method, by fusing multiple frames of point cloud data, the accuracy and reliability of terrain marking are improved, and continuous terrain change information is provided for a dynamic environment. The method comprises the steps of constructing a two-dimensional map, obtaining fusion point cloud data of each grid region, dividing the fusion point cloud data into a plurality of grid regions in a three-dimensional space, obtaining a terrain category of each grid region, and carrying out terrain marking on the grid regions based on the number of each terrain category in the fusion point cloud data. According to the method, the limitation of the traditional single-frame point cloud data in terrain labeling is solved, and the accuracy of terrain analysis and the processing efficiency of terrain labeling are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning navigation and map construction, and in particular to a two-dimensional map terrain annotation method based on multi-frame point cloud fusion data. Background Art

[0002] In the fields of agricultural pest monitoring, autonomous driving, robot navigation, etc., accurate terrain information is crucial to improving work efficiency and safety. Point cloud, as an important carrier of three-dimensional environmental information, is widely used in tasks such as terrain analysis and obstacle detection.

[0003] Traditional terrain annotation methods mainly rely on single-frame point cloud data, and achieve terrain annotation by classifying and identifying point cloud data. However, this method has limitations when dealing with complex terrain and obstacles, and it is difficult to accurately reflect the detailed features of the terrain. In addition, single-frame point cloud data cannot provide continuous terrain change information, which limits its application in dynamic environments. Summary of the invention

[0004] The present invention provides a two-dimensional map terrain annotation method and system based on multi-frame point cloud data, which improves the accuracy and reliability of terrain annotation by fusing multi-frame point cloud data and provides continuous terrain change information for dynamic environments.

[0005] The two-dimensional map terrain annotation method based on multi-frame point cloud data according to the present invention comprises:

[0006] Construct 2D maps;

[0007] Acquire fused point cloud data of each grid area; wherein the fused point cloud data is acquired based on multiple frames of point cloud data continuously collected at the current moment and multiple previous moments;

[0008] Divide the fused point cloud data into multiple grid areas in three-dimensional space;

[0009] Get the terrain category of each grid area;

[0010] Based on the number of each terrain category in the fused point cloud data, the corresponding raster area is labeled.

[0011] A two-dimensional map is constructed in the present invention, and the two-dimensional map is divided into a plurality of grid regions, each of which represents a specific region on the map and is used for subsequent point cloud data fusion and terrain annotation.

[0012] Through the grid division in the present invention, the two-dimensional map is divided into multiple areas, which provides convenience for the subsequent point cloud data processing and terrain annotation, and improves the processing efficiency and accuracy.

[0013] In the present invention, the fused point cloud data is obtained based on multiple frames of point cloud data continuously collected at the current moment and multiple previous moments, including:

[0014] Constructing a reference coordinate system based on the position of the acquisition device at the initial moment;

[0015] Based on the real-time coordinate system of each frame of point cloud data at the time of acquisition, the pose transformation matrix between the real-time coordinate system of each frame of point cloud data and the reference coordinate system is obtained;

[0016] Based on the posture transformation matrix, each frame of point cloud data in the multiple frames of point cloud data is converted to a reference coordinate system to obtain fused point cloud data; wherein the fused point cloud data includes all point cloud data after each frame of point cloud data in the multiple frames of point cloud data is converted to the reference coordinate system.

[0017] By constructing a reference coordinate system and obtaining a posture transformation matrix in the present invention, the fusion of multi-frame point cloud data in the reference coordinate system is achieved. The fused point cloud data contains richer surface information, thereby improving the accuracy and reliability of terrain analysis.

[0018] In the present invention, the posture transformation matrix includes a rotation matrix and a translation matrix T, and the rotation matrix includes a rotation matrix R in the x-axis direction. x , the rotation matrix R in the y-axis direction y And the rotation matrix R in the z-axis direction z ;

[0019] in,

[0020]

[0021]

[0022] in, is the rotation angle of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the reference coordinate system in the x-axis direction, θ is the rotation angle of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the reference coordinate system in the y-axis direction, and ρ is the rotation angle of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the reference coordinate system in the z-axis direction; T x T is the coordinate offset of the origin of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the origin of the reference coordinate system in the x-axis direction; y T is the coordinate offset of the origin of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the origin of the reference coordinate system in the y-axis direction; z It is the coordinate offset of the origin of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared with the origin of the reference coordinate system in the z-axis direction;

[0023] Any frame of point cloud data Po Any point cloud coordinate point p in or They are transformed to the reference coordinate system through the rotation matrix and translation matrix T, where

[0024] p or =[x or ,y or , z or , 1] T ,

[0025] p′ or =R x ×R y ×R z ×p or ,

[0026] p″ or =T×p′ or ;

[0027] Among them, (x or ,y or , z or ) is the point cloud coordinate point p or The spatial coordinates of or is the point cloud coordinate point p or The point cloud coordinates obtained after rotation processing, p″ or is the point cloud coordinate point p or The point cloud coordinates obtained after rotation and translation.

[0028] Through the application of the pose transformation matrix in the present invention, each frame of point cloud data can be accurately converted from its real-time coordinate system to the reference coordinate system. Since each frame of point cloud data has undergone precise rotation and translation processing, multiple frames of point cloud data can be seamlessly connected during the fusion process, reducing the fusion error caused by inconsistent coordinate systems and improving the accuracy of terrain analysis and the processing efficiency of terrain annotation.

[0029] In the present invention, the step of dividing the fused point cloud data into a plurality of grid areas in a three-dimensional space includes:

[0030] Establish a grid space based on the polar coordinate system;

[0031] Establishing a plurality of grid layer regions equidistantly in the vertical direction in the grid space; wherein the grid layer regions are fan-shaped regions;

[0032] Establishing a plurality of grid ring regions in the grid layer region at equal intervals in the radial direction; wherein the grid ring region is a fan-shaped region;

[0033] A plurality of mesh areas are established in the mesh ring area at equal intervals along the circumferential direction; wherein all the mesh areas in all the mesh ring areas together constitute the plurality of mesh areas.

[0034] By establishing a grid space based on a polar coordinate system in three-dimensional space in the present invention, the fused point cloud data is divided into multiple grid areas, making the point cloud data in each grid area more concentrated and orderly. This method significantly reduces the computational complexity of subsequent terrain analysis and improves the efficiency of data processing; according to this division method, it can be ensured that the arc length of each fan-shaped outer side is equal, reducing the calculation error caused by uneven distribution of point cloud data.

[0035] In the present invention, obtaining the terrain category of each grid area includes:

[0036] Obtain the fused point cloud data subset contained in the grid area;

[0037] Based on plane fitting of the fused point cloud data subset within the corresponding grid area, a corresponding fitting plane is obtained;

[0038] Based on the parameters of the corresponding fitting plane, the average distance between the corresponding fused point cloud data subset and the corresponding fitting plane is obtained and used as the fluctuation index;

[0039] Based on the angle between the corresponding fitting plane and the horizontal plane, a corresponding fitting plane inclination index is obtained;

[0040] Based on the distance from the corresponding grid area to the origin of the polar coordinate system, the reflection intensity index of the corresponding fused point cloud data subset is obtained.

[0041] The terrain category of the grid area is obtained based on the undulation index of the corresponding fused point cloud data subset and the corresponding fitting plane, the inclination index of the corresponding fitting plane and the reflection intensity index of the corresponding fused point cloud data subset; wherein the terrain category includes flat terrain, uneven terrain and obstacle terrain.

[0042] By combining the undulation index, the fitted plane tilt index and the reflection intensity index in the present invention, the terrain characteristics of the grid area can be more comprehensively evaluated. This method not only takes into account the geometric shape of the terrain, but also takes into account the material properties of the surface, thereby improving the accuracy of terrain classification. And because the terrain classification is based on the grid area, the processing of each grid area is independent, so multiple grid areas can be processed in parallel, improving the real-time performance of data processing.

[0043] In the present invention, the fusion point cloud data subset contained in the grid area is obtained, including:

[0044] Obtain the fused point cloud data and transform it from the reference coordinate system to the spatial polar coordinate system; wherein, any point cloud coordinate point p in the fused point cloud data i The coordinates in the reference coordinate system (x i ,y i , z i ) after conversion to the spatial polar coordinate system is

[0045] Obtain the layer height data Δh of the grid layer area, the center angle Δθ of the grid ring area, the radial width Δd of the grid ring area, and the number of grid areas R of each grid ring area β ;

[0046] Based on the layer height data Δh and any point cloud coordinate point p i The coordinate parameter z in the polar coordinate system i , get the corresponding point cloud coordinate point p i The number of layers N in the grid layer area α ;

[0047] Based on the radial width Δd of the grid ring area and the coordinate point p of any point cloud i Coordinate parameters in the polar coordinate system Get the corresponding point cloud coordinate point p i The number of rings N in the grid layer area β ;

[0048] Based on the central angle Δθ of the grid ring area and the coordinate point p of any point cloud i Coordinate parameters in the polar coordinate system Get the corresponding point cloud coordinate point p i The number of sectors in the grid layer area N γ ;

[0049] in,

[0050]

[0051] in, This is a round-up operation.

[0052] The present invention converts the fused point cloud data from the reference coordinate system to the spatial polar coordinate system, and based on the layer height data Δh, the center angle Δθ of the grid ring area, the radial width Δd and the number of grid areas per grid ring area R β , can accurately calculate the coordinates of each point cloud point p i The number of grid layer areas N α 、Number of rings β and the number of sectors N γ . It ensures that each point cloud data is accurately assigned to its corresponding grid area, improving the efficiency of point cloud data processing.

[0053] The point cloud indexes with the same layer coordinates, ring coordinates and fan coordinates are stored in the same array; after obtaining the plane parameters of each grid, the terrain can be analyzed. First, for flat ground, due to the small terrain undulation, most of the collected point cloud data should be located above the fitted plane, while for uneven roads such as grass and gravel, due to the large terrain undulation, the collected point cloud data should fluctuate near the fitted plane, so the distribution of point cloud data can be used as the basis for terrain analysis. Secondly, for special terrains such as hills and mounds, there will be a certain angle between them and the horizontal ground, so the angle between the fitting plane and the horizontal plane can also be used as the basis for terrain analysis. Finally, since the laser in the acquisition device has different permeability and reflectivity after irradiating the ground of different materials, the reflection intensity of the point cloud data collected by the acquisition device on different material roads will also be different. The reflection intensity of the acquisition device refers to the ratio of the echo power of the laser point to the transmission power. The simplified relationship between the echo power and the transmission power can be expressed by the following formula:

[0054]

[0055] Where P R Represents echo power, P E represents the transmitting power, ρ represents the reflectivity of the surface of the object, α represents the incident angle of the light, and R represents the distance. From the above formula, it can be seen that the magnitude of the echo power is inversely proportional to the incident angle and the square of the distance. The incident angle refers to the angle between the incident light and the normal line of the surface of the object. When the laser is irradiated on a horizontal road surface, the incident angle is large and its reflection intensity is usually small. When the laser is irradiated on special terrain such as grass, due to the vertical growth characteristics of grass, its incident angle is small and the reflection intensity will be greater than when irradiated on a horizontal road surface.

[0056] In the present invention, the calculation formula of the fluctuation index f is,

[0057]

[0058] The calculation formula of the fitting plane tilt index k is:

[0059]

[0060] The calculation formula of reflection intensity index T is:

[0061]

[0062] Among them, the expression of the corresponding fitting plane is Ax+By+Cz+D=0; is the unit normal vector of the corresponding fitting plane; N iRepresents the reflection intensity index of all fused point cloud data subsets in the grid area; d is the distance from the current grid area to the origin of the spatial polar coordinate system, d max Indicates the distance of the farthest grid area from the origin of the spatial polar coordinate system.

[0063] The terrain category of the grid area is obtained based on the undulation index of the corresponding fused point cloud data subset and the corresponding fitting plane, the inclination index of the corresponding fitting plane and the reflection intensity index of the corresponding fused point cloud data subset, including:

[0064] Based on the undulation index f, the plane tilt index k and the reflection intensity index T, the flatness index τ of the corresponding grid area is obtained; where τ = θ 1 k+θ 2 f+θ 3 T,θ 1 ,θ 2 and θ 3 is the preset weight value;

[0065] Based on the first preset threshold and the second preset threshold, the terrain category of the corresponding grid area is determined as flat terrain, uneven terrain or obstacle terrain; wherein, when the flatness index τ of the corresponding grid area is greater than the first preset threshold, the terrain category of the corresponding grid area is determined as obstacle terrain; wherein, when the flatness index τ of the corresponding grid area is between the first preset threshold and the second preset threshold, the terrain category of the corresponding grid area is determined as uneven terrain; wherein, when the flatness index τ of the corresponding grid area is less than the second preset threshold, the terrain category of the corresponding grid area is determined as flat terrain.

[0066] By calculating the undulation index f, the fitting plane tilt index k and the reflection intensity index T in the present invention and comprehensively evaluating the flatness of the grid area based on these indices, the terrain features can be captured more comprehensively.

[0067] In the present invention, the terrain annotation of the corresponding grid area based on the number of each terrain category in the fused point cloud data includes:

[0068] Preset the probability value of each terrain category in each grid area;

[0069] Based on the terrain category of each grid area within each grid area, the probability value of each terrain category of each grid area is updated.

[0070] By presetting the probability value of each terrain category in each grid area in the present invention, an initial, statistically based estimate is provided for terrain annotation. By dynamically updating the terrain probability, even if the terrain category judgment of a certain grid area is wrong, the correct judgment of other grid areas can still gradually correct the terrain probability of the grid area, thereby improving the robustness of terrain annotation.

[0071] In the present invention, the updating of the probability value of each terrain category of each grid area based on the terrain category of each grid area in each grid area includes:

[0072] The probability of impassability p(s=0), the probability of recommended passage p(s=1), and the probability of passability but not recommended p(s=2) of each grid are 1 / 3, and p(s≠0)=p(s=1)+p(s=2)=2 / 3;

[0073] The ratio of the two probabilities is recorded as odds (s = 0),

[0074] Based on the Bayesian formula and the ratio corresponding to p(s=0|z) under the condition that the grid measurement value z=0 at the initial moment Get the updated grid inaccessibility probability:

[0075]

[0076] Among them, p(z|s=0) represents the probability of determining that the fused point cloud data subset in the grid area is z, under the premise that the previous grid area is recommended for passage; p(z|s≠0) represents the probability of determining that the fused point cloud data subset in the grid area is z, under the premise that the previous grid area is an obstacle or is not recommended for passage;

[0077] According to the different observed values ​​z, update odds(s=0) to obtain the probability that the current grid area is impassable

[0078] Update recommended pass probability and the probability of not recommending

[0079] The maximum value among p(s=0), p(s=1) and p(s=2) is selected as the pass value of the current grid.

[0080] In this embodiment, the terrain probability of each grid area is dynamically updated by using the Bayesian formula in combination with the observed value z of each measurement. With the introduction of new data, the terrain probability distribution is gradually corrected and refined to improve the accuracy and timeliness of terrain annotation. The probability model is adopted and updated in combination with the results of multiple measurements, which can effectively reduce the impact of single measurement errors on terrain annotation. By presetting and updating the probability value of each terrain category in the grid area, the actual terrain conditions of the grid area can be more accurately reflected. The terrain category with the largest probability value is selected as the pass value, which provides reliable terrain information for navigation and path planning, and improves the accuracy and safety of navigation and path planning.

[0081] The present invention also provides a two-dimensional map terrain annotation system based on multi-frame point cloud fusion data, which is applied to a two-dimensional map terrain annotation method based on multi-frame point cloud fusion data, and the system comprises:

[0082] Two-dimensional map construction module: used to construct a two-dimensional map and divide the constructed two-dimensional map into multiple grid areas.

[0083] Point cloud data acquisition module: connected to the two-dimensional map construction module, used to obtain the fused point cloud data of each grid area.

[0084] Grid area division module: connected to the fused point cloud data acquisition module, used to divide the fused point cloud data into multiple grid areas in three-dimensional space.

[0085] Terrain category acquisition module: connected to the grid area division module, used to obtain the terrain category of each grid area.

[0086] Terrain annotation module: connected to the terrain category acquisition module, it is used to perform terrain annotation on the corresponding grid area based on the number of each terrain category in the fused point cloud data. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 A schematic flow chart of a preferred embodiment of a two-dimensional map terrain annotation method based on multi-frame point cloud fusion data provided by the present invention.

[0088] Figure 2 A schematic diagram of a process for obtaining fused point cloud data based on multi-frame point cloud data continuously collected at the current moment and multiple previous moments in another preferred embodiment of a two-dimensional map terrain annotation method based on multi-frame point cloud fusion data provided by the present invention

[0089] Figure 3 A schematic flow chart of dividing fused point cloud data into multiple grid areas in three-dimensional space in another preferred embodiment of the two-dimensional map terrain annotation method based on multi-frame point cloud fusion data provided by the present invention.

[0090] Figure 4 A schematic diagram of a process for obtaining the terrain category of each grid area in another preferred embodiment of the two-dimensional map terrain annotation method based on multi-frame point cloud fusion data provided by the present invention.

[0091] Figure 5 A schematic diagram of a process for terrain annotation of a corresponding grid area based on the number of each terrain category in the fused point cloud data in one preferred embodiment of a two-dimensional map terrain annotation method based on multi-frame point cloud fusion data provided by the present invention.

[0092] Figure 6 A schematic flow chart of a preferred embodiment of a two-dimensional map terrain annotation system based on multi-frame point cloud fusion data provided by the present invention. DETAILED DESCRIPTION

[0093] In order to further understand the content of the present invention, the present invention is described in detail in conjunction with the embodiments. It should be understood that the embodiments are only for explaining the present invention and are not intended to limit it.

[0094] Example 1

[0095] First, see Figure 1 A preferred embodiment of the two-dimensional map terrain annotation method based on multi-frame point cloud fusion data provided by the present invention includes the following steps:

[0096] Construct 2D maps;

[0097] Acquire fused point cloud data of each grid area; wherein the fused point cloud data is acquired based on multiple frames of point cloud data continuously collected at the current moment and multiple previous moments;

[0098] Divide the fused point cloud data into multiple grid areas in three-dimensional space;

[0099] Get the terrain category of each grid area;

[0100] Based on the number of each terrain category in the fused point cloud data, the corresponding raster area is labeled.

[0101] In this embodiment, a two-dimensional map is constructed and divided into a plurality of grid regions, each of which represents a specific area on the map and is used for subsequent point cloud data fusion and terrain annotation.

[0102] Through the grid division in this embodiment, the two-dimensional map is divided into multiple areas, which provides convenience for subsequent point cloud data processing and terrain annotation, and improves processing efficiency and accuracy.

[0103] See also Figure 2 In this embodiment, the fused point cloud data is obtained based on multiple frames of point cloud data continuously collected at the current moment and multiple previous moments, including:

[0104] Constructing a reference coordinate system based on the position of the acquisition device at the initial moment;

[0105] Based on the real-time coordinate system of each frame of point cloud data at the time of acquisition, the pose transformation matrix between the real-time coordinate system of each frame of point cloud data and the reference coordinate system is obtained;

[0106] Based on the posture transformation matrix, each frame of point cloud data in the multiple frames of point cloud data is converted to a reference coordinate system to obtain fused point cloud data; wherein the fused point cloud data includes all point cloud data after each frame of point cloud data in the multiple frames of point cloud data is converted to the reference coordinate system.

[0107] By constructing a reference coordinate system and obtaining a pose transformation matrix in this embodiment, the fusion of multi-frame point cloud data in the reference coordinate system is achieved. The fused point cloud data contains richer surface information, thereby improving the accuracy and reliability of terrain analysis.

[0108] In this embodiment, the posture transformation matrix includes a rotation matrix and a translation matrix T, and the rotation matrix includes a rotation matrix R in the x-axis direction. x , the rotation matrix R in the y-axis direction y And the rotation matrix R in the z-axis direction z ;

[0109] in,

[0110]

[0111] in, is the rotation angle of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the reference coordinate system in the x-axis direction, θ is the rotation angle of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the reference coordinate system in the y-axis direction, and ρ is the rotation angle of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the reference coordinate system in the z-axis direction; T x T is the coordinate offset of the origin of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the origin of the reference coordinate system in the x-axis direction; y T is the coordinate offset of the origin of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the origin of the reference coordinate system in the y-axis direction; z It is the coordinate offset of the origin of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared with the origin of the reference coordinate system in the z-axis direction;

[0112] Any frame of point cloud data P o Any point cloud coordinate point p in or They are transformed to the reference coordinate system through the rotation matrix and translation matrix T, where

[0113] p or =[x or ,y or , z or , 1] T ,

[0114] p′ or =R x ×R y ×Rz ×p or ,

[0115] p″ or =T×p′ or ;

[0116] Among them, (x or ,y or , z or ) is the point cloud coordinate point p or The spatial coordinates of or is the point cloud coordinate point p or The point cloud coordinates obtained after rotation processing, p″ or is the point cloud coordinate point p or The point cloud coordinates obtained after rotation and translation.

[0117] Through the application of the pose transformation matrix in this embodiment, each frame of point cloud data can be accurately converted from its real-time coordinate system to the reference coordinate system. Since each frame of point cloud data has undergone precise rotation and translation processing, multiple frames of point cloud data can be seamlessly connected during the fusion process, reducing the fusion error caused by inconsistent coordinate systems and improving the accuracy of terrain analysis and the processing efficiency of terrain annotation.

[0118] See also Figure 3 In this embodiment, the fused point cloud data is divided into a plurality of grid areas in the three-dimensional space, including:

[0119] Establish a grid space based on the polar coordinate system;

[0120] Establishing a plurality of grid layer regions equidistantly in the vertical direction in the grid space; wherein the grid layer regions are fan-shaped regions;

[0121] Establishing a plurality of grid ring regions in the grid layer region at equal intervals in the radial direction; wherein the grid ring region is a fan-shaped region;

[0122] A plurality of mesh areas are established in the mesh ring area at equal intervals along the circumferential direction; wherein all the mesh areas in all the mesh ring areas together constitute the plurality of mesh areas.

[0123] By establishing a grid space based on a polar coordinate system in three-dimensional space in this embodiment, the fused point cloud data is divided into multiple grid areas, making the point cloud data in each grid area more concentrated and orderly. This method significantly reduces the computational complexity of subsequent terrain analysis and improves the efficiency of data processing; according to this division method, it can be ensured that the arc length of each sector is equal, reducing the calculation error caused by uneven distribution of point cloud data.

[0124] See also Figure 3In this embodiment, obtaining the terrain category of each grid area includes:

[0125] Obtain the fused point cloud data subset contained in the grid area;

[0126] Based on plane fitting of the fused point cloud data subset within the corresponding grid area, a corresponding fitting plane is obtained;

[0127] Based on the parameters of the corresponding fitting plane, the average distance between the corresponding fused point cloud data subset and the corresponding fitting plane is obtained and used as the fluctuation index;

[0128] Based on the angle between the corresponding fitting plane and the horizontal plane, a corresponding fitting plane inclination index is obtained;

[0129] Based on the distance from the corresponding grid area to the origin of the polar coordinate system, the reflection intensity index of the corresponding fused point cloud data subset is obtained.

[0130] The terrain category of the grid area is obtained based on the undulation index of the corresponding fused point cloud data subset and the corresponding fitting plane, the inclination index of the corresponding fitting plane and the reflection intensity index of the corresponding fused point cloud data subset; wherein the terrain category includes flat terrain, uneven terrain and obstacle terrain.

[0131] By combining the undulation index, the fitted plane tilt index and the reflection intensity index in this embodiment, the terrain characteristics of the grid area can be more comprehensively evaluated. This method not only takes into account the geometric shape of the terrain, but also takes into account the material properties of the surface, thereby improving the accuracy of terrain classification. And because the terrain classification is based on the grid area, the processing of each grid area is independent, so multiple grid areas can be processed in parallel, improving the real-time performance of data processing.

[0132] In this embodiment, the acquisition of the fused point cloud data subset contained in the grid area includes:

[0133] Obtain the fused point cloud data and transform it from the reference coordinate system to the spatial polar coordinate system; wherein, any point cloud coordinate point p in the fused point cloud data i The coordinates in the reference coordinate system (x i ,y i , z i ) after conversion to the spatial polar coordinate system is

[0134] Obtain the layer height data Δh of the grid layer area, the center angle Δθ of the grid ring area, the radial width Δd of the grid ring area, and the number of grid areas R of each grid ring area β ;

[0135] Based on the layer height data Δh and any point cloud coordinate point p iThe coordinate parameter z in the polar coordinate system i , get the corresponding point cloud coordinate point p i The number of layers N in the grid layer area α ;

[0136] Based on the radial width Δd of the grid ring area and the coordinate point p of any point cloud i Coordinate parameters in the polar coordinate system Get the corresponding point cloud coordinate point p i The number of rings N in the grid layer area β ;

[0137] Based on the central angle Δθ of the grid ring area and the coordinate point p of any point cloud i Coordinate parameters in the polar coordinate system Get the corresponding point cloud coordinate point p i The number of sectors in the grid layer area N γ ;

[0138] in,

[0139]

[0140] in, This is a round-up operation.

[0141] In this embodiment, the fused point cloud data is converted from the reference coordinate system to the spatial polar coordinate system, and based on the layer height data Δh, the center angle Δθ of the grid ring area, the radial width Δd, and the number of grid areas per grid ring area R β , can accurately calculate the coordinates of each point cloud point p i The number of grid layer areas N α 、Number of rings β and the number of sectors N γ . It ensures that each point cloud data is accurately assigned to its corresponding grid area, improving the efficiency of point cloud data processing.

[0142] The point cloud indexes with the same layer coordinates, ring coordinates and fan coordinates are stored in the same array; after obtaining the plane parameters of each grid, the terrain can be analyzed. First, for flat ground, due to the small terrain undulation, most of the collected point cloud data should be located above the fitted plane, while for uneven roads such as grass and gravel, due to the large terrain undulation, the collected point cloud data should fluctuate near the fitted plane, so the distribution of point cloud data can be used as the basis for terrain analysis. Secondly, for special terrains such as hills and mounds, there will be a certain angle between them and the horizontal ground, so the angle between the fitting plane and the horizontal plane can also be used as the basis for terrain analysis. Finally, since the laser in the acquisition device has different permeability and reflectivity after irradiating the ground of different materials, the reflection intensity of the point cloud data collected by the acquisition device on different material roads will also be different. The reflection intensity of the acquisition device refers to the ratio of the echo power of the laser point to the transmission power. The simplified relationship between the echo power and the transmission power can be expressed by the following formula:

[0143]

[0144] Where P R Represents echo power, P E represents the transmitting power, ρ represents the reflectivity of the surface of the object, α represents the incident angle of the light, and R represents the distance. From the above formula, it can be seen that the magnitude of the echo power is inversely proportional to the incident angle and the square of the distance. The incident angle refers to the angle between the incident light and the normal line of the surface of the object. When the laser is irradiated on a horizontal road surface, the incident angle is large and its reflection intensity is usually small. When the laser is irradiated on special terrain such as grass, due to the vertical growth characteristics of grass, its incident angle is small and the reflection intensity will be greater than when irradiated on a horizontal road surface.

[0145] In this embodiment, the calculation formula of the fluctuation index f is:

[0146]

[0147] The calculation formula of the fitting plane tilt index k is:

[0148]

[0149] The calculation formula of reflection intensity index T is:

[0150]

[0151] Among them, the expression of the corresponding fitting plane is Ax+By+Cz+D=0; is the unit normal vector of the corresponding fitting plane; N iRepresents the reflection intensity index of all fused point cloud data subsets in the grid area; d is the distance from the current grid area to the origin of the spatial polar coordinate system, d max Indicates the distance of the farthest grid area from the origin of the spatial polar coordinate system.

[0152] The terrain category of the grid area is obtained based on the undulation index of the corresponding fused point cloud data subset and the corresponding fitting plane, the inclination index of the corresponding fitting plane and the reflection intensity index of the corresponding fused point cloud data subset, including:

[0153] Based on the undulation index f, the plane tilt index k and the reflection intensity index T, the flatness index τ of the corresponding grid area is obtained; where τ = θ 1 k+θ 2 f+θ 3 T,θ 1 ,θ 2 and θ 3 is the preset weight value;

[0154] Based on the first preset threshold and the second preset threshold, the terrain category of the corresponding grid area is determined as flat terrain, uneven terrain or obstacle terrain; wherein, when the flatness index τ of the corresponding grid area is greater than the first preset threshold, the terrain category of the corresponding grid area is determined as obstacle terrain; wherein, when the flatness index τ of the corresponding grid area is between the first preset threshold and the second preset threshold, the terrain category of the corresponding grid area is determined as uneven terrain; wherein, when the flatness index τ of the corresponding grid area is less than the second preset threshold, the terrain category of the corresponding grid area is determined as flat terrain.

[0155] By calculating the undulation index f, the fitting plane tilt index k and the reflection intensity index T in this embodiment, and comprehensively evaluating the flatness of the grid area based on these indices, the terrain features can be captured more comprehensively.

[0156] See also Figure 4 In this embodiment, the terrain annotation of the corresponding grid area based on the number of each terrain category in the fused point cloud data includes:

[0157] Preset the probability value of each terrain category in each grid area;

[0158] Based on the terrain category of each grid area within each grid area, the probability value of each terrain category of each grid area is updated.

[0159] By presetting the probability value of each terrain category in each grid area in this embodiment, an initial, statistically based estimate is provided for terrain annotation. By dynamically updating the terrain probability, even if the terrain category of a certain grid area is judged incorrectly, the correct judgment of other grid areas can still gradually correct the terrain probability of the grid area, thereby improving the robustness of terrain annotation.

[0160] In this embodiment, the updating of the probability value of each terrain category of each grid area based on the terrain category of each grid area in each grid area includes:

[0161] The probability of impassability p(s=0), the probability of recommended passage p(s=1), and the probability of passability but not recommended p(s=2) of each grid are 1 / 3, and p(s≠0)=p(s=1)+p(s=2)=2 / 3;

[0162] The ratio of the two probabilities is recorded as odds (s = 0),

[0163] Based on the Bayesian formula and the ratio corresponding to p(s=0|z) under the condition that the grid measurement value z=0 at the initial moment Get the updated grid inaccessibility probability:

[0164]

[0165] Among them, p(z|s=0) represents the probability of determining that the fused point cloud data subset in the grid area is z, under the premise that the previous grid area is recommended for passage; p(z|s≠0) represents the probability of determining that the fused point cloud data subset in the grid area is z, under the premise that the previous grid area is an obstacle or is not recommended for passage;

[0166] According to the different observed values ​​z, update odds(s=0) to obtain the probability that the current grid area is impassable

[0167] Update recommended pass probability and the probability of not recommending

[0168] The maximum value among p(s=0), p(s=1) and p(s=2) is selected as the pass value of the current grid.

[0169] In this embodiment, the terrain probability of each grid area is dynamically updated by using the Bayesian formula in combination with the observed value z of each measurement. With the introduction of new data, the terrain probability distribution is gradually corrected and refined to improve the accuracy and timeliness of terrain annotation. The probability model is adopted and updated in combination with the results of multiple measurements, which can effectively reduce the impact of single measurement errors on terrain annotation. By presetting and updating the probability value of each terrain category in the grid area, the actual terrain conditions of the grid area can be more accurately reflected. The terrain category with the largest probability value is selected as the pass value, which provides reliable terrain information for navigation and path planning, and improves the accuracy and safety of navigation and path planning.

[0170] See also Figure 5 According to a second aspect of this embodiment, a two-dimensional map terrain annotation system based on multi-frame point cloud fusion data is provided, which is applied to a two-dimensional map terrain annotation method based on multi-frame point cloud fusion data, and the system includes:

[0171] Two-dimensional map construction module: used to construct a two-dimensional map and divide the constructed two-dimensional map into multiple grid areas.

[0172] Point cloud data acquisition module: connected to the two-dimensional map construction module, used to obtain the fused point cloud data of each grid area.

[0173] Grid area division module: connected to the fused point cloud data acquisition module, used to divide the fused point cloud data into multiple grid areas in three-dimensional space.

[0174] Terrain category acquisition module: connected to the grid area division module, used to obtain the terrain category of each grid area.

[0175] Terrain annotation module: connected to the terrain category acquisition module, it is used to perform terrain annotation on the corresponding grid area based on the number of each terrain category in the fused point cloud data.

[0176] It is easy to understand that those skilled in the art can combine, split, reorganize, etc. the embodiments of the present application to obtain other embodiments based on one or several embodiments provided in the present application, and these embodiments do not exceed the protection scope of the present application.

[0177] The present invention and its implementation methods are described schematically above, and the description is not restrictive. The embodiments shown in the embodiments are only part of the implementation methods of the present invention, and the actual structure is not limited thereto. Therefore, if a person skilled in the art is inspired by the embodiments and designs a structure and an implementation method similar to the technical solution without creativity without departing from the purpose of the invention, they should all fall within the protection scope of the present invention.

Claims

1. A two-dimensional map terrain annotation method based on multi-frame point cloud data, comprising: Constructing a two-dimensional map; wherein the constructed two-dimensional map is divided into a plurality of grid areas; Acquire fused point cloud data of each grid area; wherein the fused point cloud data is acquired based on multiple frames of point cloud data continuously collected at the current moment and multiple previous moments; Divide the fused point cloud data into multiple grid areas in three-dimensional space; Get the terrain category of each grid area; Based on the number of each terrain category in the fused point cloud data, the corresponding raster area is labeled.

2. The two-dimensional map terrain annotation method based on multi-frame point cloud fusion data according to claim 1 is characterized by: The multi-frame point cloud data is acquired by an acquisition device at different spatial positions at consecutive moments; The fused point cloud data is obtained based on multiple frames of point cloud data continuously collected at the current moment and multiple previous moments, including: Constructing a reference coordinate system based on the position of the acquisition device at the initial moment; Based on the real-time coordinate system of each frame of point cloud data at the time of acquisition, the pose transformation matrix between the real-time coordinate system of each frame of point cloud data and the reference coordinate system is obtained; Based on the posture transformation matrix, each frame of point cloud data in the multiple frames of point cloud data is converted to a reference coordinate system to obtain fused point cloud data; wherein the fused point cloud data includes all point cloud data after each frame of point cloud data in the multiple frames of point cloud data is converted to the reference coordinate system.

3. The two-dimensional map terrain annotation method based on multi-frame point cloud fusion data according to claim 2 is characterized in that: The pose transformation matrix includes the rotation matrix and the translation matrix T. The rotation matrix includes the rotation matrix R in the x-axis direction. x , the rotation matrix R in the y-axis direction y And the rotation matrix R in the z-axis direction z ; in, in, is the rotation angle of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the reference coordinate system in the x-axis direction, θ is the rotation angle of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the reference coordinate system in the y-axis direction, and ρ is the rotation angle of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the reference coordinate system in the z-axis direction; T x T is the coordinate offset of the origin of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the origin of the reference coordinate system in the x-axis direction; y T is the coordinate offset of the origin of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared to the origin of the reference coordinate system in the y-axis direction; z It is the coordinate offset of the origin of the real-time coordinate system of each frame of point cloud data at the time of acquisition compared with the origin of the reference coordinate system in the z-axis direction; Any frame of point cloud data P o Any point cloud coordinate point p in or They are transformed to the reference coordinate system through the rotation matrix and translation matrix T, where p or =[x or ,y or ,z or ,1] T , p ′ or =R x ×R y ×R z ×p or , p ′ o ′ r =T×p ′ or ; Among them, (x or ,y or , z or ) is the point cloud coordinate point p or The spatial coordinates of ′ or is the point cloud coordinate point p or The point cloud coordinates obtained after rotation processing, p ′ o ′ r is the point cloud coordinate point p or The point cloud coordinates obtained after rotation and translation.

4. The two-dimensional map terrain annotation method based on multi-frame point cloud fusion data according to claim 2 is characterized in that: The step of dividing the fused point cloud data into a plurality of grid areas in the three-dimensional space includes: Establish a grid space based on the polar coordinate system; Establishing a plurality of grid layer regions equidistantly in the vertical direction in the grid space; wherein the grid layer regions are fan-shaped regions; Establishing a plurality of grid ring regions in the grid layer region at equal distances in the radial direction; wherein the grid ring region is a fan-shaped region; A plurality of mesh areas are established in the mesh ring area at equal intervals along the circumferential direction; wherein all the mesh areas in all the mesh ring areas together constitute the plurality of mesh areas.

5. The two-dimensional map terrain annotation method based on multi-frame point cloud fusion data according to claim 4 is characterized in that: The method of obtaining the terrain category of each grid area includes: Obtain the fused point cloud data subset contained in the grid area; Based on plane fitting of the fused point cloud data subset within the corresponding grid area, a corresponding fitting plane is obtained; Based on the parameters of the corresponding fitting plane, the average distance between the corresponding fused point cloud data subset and the corresponding fitting plane is obtained and used as the fluctuation index; Based on the angle between the corresponding fitting plane and the horizontal plane, a corresponding fitting plane inclination index is obtained; Based on the distance from the corresponding grid area to the origin of the polar coordinate system, the reflection intensity index of the corresponding fused point cloud data subset is obtained; The terrain category of the grid area is obtained based on the undulation index of the corresponding fused point cloud data subset and the corresponding fitting plane, the inclination index of the corresponding fitting plane and the reflection intensity index of the corresponding fused point cloud data subset; wherein the terrain category includes flat terrain, uneven terrain and obstacle terrain.

6. The two-dimensional map terrain annotation method based on multi-frame point cloud fusion data according to claim 5 is characterized in that: The obtaining of the fused point cloud data subset contained in the grid area includes: Obtain the fused point cloud data and transform it from the reference coordinate system to the spatial polar coordinate system; wherein, any point cloud coordinate point p in the fused point cloud data i The coordinates in the reference coordinate system (x i ,y i , z i ) after conversion to the spatial polar coordinate system is Obtain the layer height data Δh of the grid layer area, the center angle Δθ of the grid ring area, the radial width Δd of the grid ring area, and the number of grid areas R of each grid ring area β ; Based on the layer height data Δh and any point cloud coordinate point p i The coordinate parameter z in the polar coordinate system of space i , get the corresponding point cloud coordinate point p i The number of layers N in the grid layer area α ; Based on the radial width Δd of the grid ring area and the coordinate point p of any point cloud i Coordinate parameters in the polar coordinate system Get the corresponding point cloud coordinate point p i The number of rings N in the grid layer area β ; Based on the central angle Δθ of the grid ring area and the coordinate point p of any point cloud i Coordinate parameters in the polar coordinate system Get the corresponding point cloud coordinate point p i The number of sectors in the grid layer area N γ ; in, in, This is a round-up operation.

7. The two-dimensional map terrain annotation method based on multi-frame point cloud fusion data according to claim 6 is characterized by: The calculation formula of the fluctuation index f is: The calculation formula of the fitting plane tilt index k is: The calculation formula of reflection intensity index T is: Among them, the expression of the corresponding fitting plane is Ax+By+Cz+D=0; is the unit normal vector of the corresponding fitting plane; N i Represents the reflection intensity index of all fused point cloud data subsets in the grid area; d is the distance from the current grid area to the origin of the spatial polar coordinate system, d max Indicates the distance between the farthest grid area and the origin of the spatial polar coordinate system; The terrain category of the grid area is obtained based on the undulation index of the corresponding fused point cloud data subset and the corresponding fitting plane, the inclination index of the corresponding fitting plane and the reflection intensity index of the corresponding fused point cloud data subset, including: Based on the undulation index f, the plane tilt index k and the reflection intensity index T, the flatness index τ of the corresponding grid area is obtained; wherein τ=θ1k+θ2f+θ3T, θ1, θ2 and θ3 are preset weight values; Based on the first preset threshold and the second preset threshold, the terrain category of the corresponding grid area is determined as flat terrain, uneven terrain or obstacle terrain; wherein, when the flatness index τ of the corresponding grid area is greater than the first preset threshold, the terrain category of the corresponding grid area is determined as obstacle terrain; wherein, when the flatness index τ of the corresponding grid area is between the first preset threshold and the second preset threshold, the terrain category of the corresponding grid area is determined as uneven terrain; wherein, when the flatness index τ of the corresponding grid area is less than the second preset threshold, the terrain category of the corresponding grid area is determined as flat terrain.

8. The two-dimensional map terrain annotation method based on multi-frame point cloud fusion data according to claim 1 is characterized by: The method of annotating the corresponding grid area based on the number of each terrain category in the fused point cloud data includes: The probability value of each terrain category in each grid area is preset; Based on the terrain category of each grid area within each grid area, the probability value of each terrain category of each grid area is updated.

9. The two-dimensional map terrain annotation method based on multi-frame point cloud fusion data according to claim 8, characterized in that: The updating of the probability value of each terrain category of each grid area based on the terrain category of each grid area in each grid area includes: The probability of impassability p(s=0), the probability of recommended passage p(s=1), and the probability of passability but not recommended p(s=2) of each grid are 1 / 3, and p(s≠0)=p(s=1)+p(s=2)=2 / 3; The ratio of the two probabilities is recorded as odds (s = 0), Based on the Bayesian formula and the ratio corresponding to p(s=0|z) under the condition that the grid measurement value z=0 at the initial moment Get the updated grid inaccessibility probability: Among them, p(z|s=0) represents the probability of determining that the fused point cloud data subset in the grid area is z, under the premise that the previous grid area is recommended for passage; p(z|s≠0) represents the probability of determining that the fused point cloud data subset in the grid area is z, under the premise that the previous grid area is an obstacle or not recommended for passage; According to the different observed values ​​z, update odds(s=0) to obtain the probability that the current grid area is impassable Update recommended pass probability and the probability of not recommending The maximum value among p(s=0), p(s=1) and p(s=2) is selected as the pass value of the current grid.

10. A two-dimensional map terrain annotation system based on multi-frame point cloud fusion data, characterized in that: include: Two-dimensional map construction module: used to construct a two-dimensional map and divide the constructed two-dimensional map into multiple grid areas; Point cloud data acquisition module: connected to the two-dimensional map construction module, used to obtain the fused point cloud data of each grid area; Grid area division module: connected to the fusion point cloud data acquisition module, used to divide the fusion point cloud data into multiple grid areas in three-dimensional space; Terrain category acquisition module: connected to the grid area division module, used to obtain the terrain category of each grid area; Terrain annotation module: connected to the terrain category acquisition module, it is used to perform terrain annotation on the corresponding grid area based on the number of each terrain category in the fused point cloud data.

Images