Cross-layer path planning method and system based on multilayer elevation map

Abstract

The invention provides a cross-layer path planning method and system based on a multilayer elevation map, and the method comprises the steps: obtaining three-dimensional point cloud data of a multilayer environment, carrying out the adaptive layering of the three-dimensional point cloud data, and obtaining the number of layers of a point cloud group correspondingly covering all elevation information; constructing an elevation map of each layer of point cloud group; calculating the height difference between the current grid and the adjacent grid to obtain the trafficability of the current grid, analyzing the terrain, and optimizing the elevation map; identifying an effective point at the junction of each layer as a passing gateway point; and performing cross-layer path search based on the constructed multi-layer elevation map and the passage gateway point. According to the method, seamless path planning of cross-layer scenes such as stairs and slopes is realized based on the multi-layer elevation map and the layering strategy, and the problem of path breakage during interlayer transition of a traditional method is solved.

Description

Technical Field

[0001] The present invention relates to the field of path planning technology, and more particularly to a cross-layer path planning method and system based on a multi-layer elevation map. In particular, the present invention relates to a cross-layer path planning method based on a multi-layer elevation map and gateways between adjacent layers. Background Art

[0002] Cross-layer path planning involves 3D environments. The map representation of cross-layer scenes will affect the performance of the path planning algorithm. Using an appropriate map representation can help improve scene processing efficiency and planning speed. However, 3D environments generally use voxel maps to discretize space into structured 3D grids. Although this is easy to construct and process, it is difficult to ensure high map processing efficiency at high resolutions. In addition, since quadruped ground robots are constrained by the ground and do not care about map information in the free 3D space, there is a huge waste of voxel maps, resulting in low computational efficiency. In addition, traditional path planning algorithms (such as A*, Dijkstra, RRT, etc.) usually search for the optimal path based on a single-layer two-dimensional map, and their application scenarios are limited to planar environments. However, in multi-layer stereoscopic spaces, traditional methods need to simplify multi-layer maps into single-layer processing, ignoring the connectivity between layers, and cannot directly generate cross-layer optimal paths.

[0003] Patent document CN116608862A discloses a path planning and navigation method, storage medium, and electronic device for a multi-layer parking lot, relating to the field of navigation technology. The method comprises: obtaining a three-dimensional road network based on a vector electronic map of the target parking lot; and obtaining target path information between a starting point and an end point set on the three-dimensional road network based on a path planning algorithm. Using the vector electronic map of the parking lot, a three-dimensional road network conforming to a multi-layer structure is constructed for use in navigation path information planning. Because the movement cost between nodes connecting different single-layer road networks is set to be greater than the movement cost between adjacent nodes within each single-layer road network, cross-layer path planning can determine whether cross-layer movement occurs based on the movement cost of the identified nodes, eliminating the need for manual switching. This allows for continuous real-time navigation within a multi-layer structure based on the planned target path information.

[0004] However, patent document CN116608862A has obvious limitations in terms of elevation information processing. Its solution is limited to simple elevation differences between different floors and lacks the ability to analyze the elevation characteristics of quadruped robots. In particular, this method is only designed for vehicle navigation and cannot meet the evaluation requirements of quadruped robots for three-dimensional traffic elements such as step height, slope angle, and ground flatness in complex terrain. The present invention constructs an elevation map for quadruped robots, which not only records the elevation change characteristics of the three-dimensional environment, but also combines the quadruped kinematic model to analyze the terrain passability (including maximum obstacle crossing height, safe slope, etc.), providing a three-dimensional path planning solution for quadruped robots, significantly improving their mobility efficiency and safety in multi-layer complex environments. Summary of the Invention

[0005] In view of the defects in the prior art, the purpose of the present invention is to provide a cross-layer path planning method and system based on multi-layer elevation maps.

[0006] According to the present invention, a cross-layer path planning method based on a multi-layer elevation map is provided, comprising:

[0007] Step S1: Acquire three-dimensional point cloud data of a multi-layer environment, and adaptively layer the three-dimensional point cloud data to obtain the number of layers corresponding to the point cloud group covering all elevation information;

[0008] Step S2: constructing the elevation map of each layer of point cloud group;

[0009] Step S3: Calculate the height difference between the current grid and the adjacent grid to obtain the passability of the current grid, analyze the terrain, and optimize the elevation map;

[0010] Step S4: identifying valid points at the intersection of each layer as gateway points;

[0011] Step S5: Based on the constructed multi-layer elevation map and the gateway points, a cross-layer path search is performed.

[0012] Preferably, the step S1 includes:

[0013] Step S1.1: Segment the point cloud data according to a fixed spacing to obtain N layers of point clouds;

[0014] Step S1.2: Count the number of point clouds in each layer and calculate the corresponding average value. The formula is as follows:

[0015]

[0016] Among them, num 平均 is the average value, N is the N-layer point cloud in step S1.1, num i is the number of point clouds in the i-th layer;

[0017] Step S1.3: Identify the layer with a point cloud number greater than the average as a plane layer, and obtain the plane layer index number m.

[0018] Step S1.4: Calculate the elevation of each plane layer based on the lowest value h0 in the z-axis direction of the multi-layer environment, the plane layer index m, and the plane interval. The formula is as follows:

[0019] h=h0+m*1

[0020] Step S1.5: Take the average elevation value of the adjacent plane layers and calculate the average value. The elevation surface where the average value is located can be used as the tangent plane. There are m-1 tangent planes in the m-layer plane layer. Point clouds are allocated according to the tangent planes, and finally the simplest point cloud group of m layers that covers all elevation information is obtained.

[0021] Preferably, in step S2, the point cloud data of a single layer is distributed to each grid of the grid map, and the two-dimensional coordinate corresponding to each grid has a fixed height value. By storing the height value of each grid, a single-layer elevation layer is obtained. The searched 2D path can be naturally mapped to a 3D path according to the corresponding elevation layer. By combining m layers of elevation layers, the required multi-layer elevation map can be obtained.

[0022] Preferably, step S3 includes obtaining the passability of the current grid by calculating the height difference between the current grid and the adjacent grid, analyzing the terrain, and when the height difference exceeds the vertical passability of the four-legged dog, dividing the current grid into inaccessible areas and performing appropriate expansion processing on the inaccessible areas.

[0023] Preferably, step S4 includes:

[0024] Step S4.1: store all point clouds intersecting the section plane as possible gateway groups;

[0025] Step S4.2: Construct a virtual cuboid at each possible gateway, and store the point cloud of the points 1 meter away from the current point in the positive and negative directions of the z axis;

[0026] Step S4.3: Perform a preliminary screening. For the actual points with values ​​in the virtual cuboid, calculate the height difference s between the lowest point cloud and the highest point cloud. If s is less than or equal to the vertical distance traveled by the four-legged dog, then the possible gateway point corresponding to the virtual cuboid is selected as the candidate gateway point.

[0027] Step S4.4: Perform fine screening to filter out outliers by calculating the standard deviation and select the final gateway point group.

[0028] Step S4.5: Calculate the coordinate average of the final gateway point group to obtain the gateway point of the tangent plane.

[0029] According to the present invention, a cross-layer path planning system based on a multi-layer elevation map is provided, comprising:

[0030] Module M1: Acquire three-dimensional point cloud data of a multi-layer environment, and adaptively layer the three-dimensional point cloud data to obtain the number of layers corresponding to the point cloud group covering all elevation information;

[0031] Module M2: Construct elevation map of each layer of point cloud group;

[0032] Module M3: Calculate the height difference between the current grid and the adjacent grid to obtain the accessibility of the current grid, analyze the terrain, and optimize the elevation map;

[0033] Module M4: Identify the valid points at the intersection of each layer as the gateway points;

[0034] Module M5: Perform cross-layer path search based on the constructed multi-layer elevation map and access gateway points.

[0035] Preferably, the module M1 includes:

[0036] Module M1.1: Segment the point cloud data according to a fixed spacing to obtain N layers of point cloud;

[0037] Module M1.2: Count the number of point clouds in each layer and calculate the corresponding average value. The formula is as follows:

[0038]

[0039] Among them, num 平均 is the average value, N is the N-layer point cloud in module M1.1, num i is the number of point clouds in the i-th layer;

[0040] Module M1.3: Identify the layer with a point cloud number greater than the average as a plane layer, and obtain the plane layer index number m.

[0041] Module M1.4: Calculate the elevation of each plane layer based on the lowest value h0 in the z-axis direction of the multi-layer environment, the plane layer index m, and the plane interval. The formula is as follows:

[0042] h=h0+m*1

[0043] Module M1.5: Take the average elevation value of adjacent plane layers and use the elevation surface where the average value is located as the tangent plane. There are m-1 tangent planes in m-layer plane layers. Point clouds are allocated according to the tangent planes, and finally the simplest point cloud group covering all elevation information in m layers is obtained.

[0044] Preferably, in the module M2, the point cloud data of a single layer is distributed to each grid of the grid map. The two-dimensional coordinate corresponding to each grid has a fixed height value. By storing the height value of each grid, a single-layer elevation layer is obtained. The searched 2D path can be naturally mapped to a 3D path according to the corresponding elevation layer. By combining the m layers of elevation layers, the required multi-layer elevation map can be obtained.

[0045] Preferably, the module M3 includes obtaining the passability of the current grid by calculating the height difference between the current grid and the adjacent grid, analyzing the terrain, and when the height difference exceeds the vertical passability of the four-legged dog, dividing the current grid into inaccessible areas and performing appropriate expansion processing on the inaccessible areas.

[0046] Preferably, the module M4 includes:

[0047] Module M4.1: Store all point clouds intersecting the section plane as possible gateway groups;

[0048] Module M4.2: Construct a virtual cuboid at each possible gateway, and store the point cloud of the points 1 meter away from the current point in the positive and negative directions of the z axis.

[0049] Module M4.3: Perform a preliminary screening. For the actual points with values ​​in the virtual cuboid, calculate the height difference s between the lowest point cloud and the highest point cloud. If s is less than or equal to the vertical distance traveled by the four-legged dog, then the possible gateway point corresponding to the virtual cuboid is selected as the candidate gateway point.

[0050] Module M4.4: Perform fine screening, filter out outliers by calculating the standard deviation, and select the final gateway point group.

[0051] Module M4.5: Calculate the coordinate average of the final gateway point group to obtain the gateway point of the tangent plane.

[0052] Compared with the prior art, the present invention has the following beneficial effects:

[0053] 1. Based on multi-layer elevation maps and layering strategies, the present invention realizes seamless path planning for cross-layer scenes such as stairs and slopes, solving the path breakage problem of traditional methods during inter-layer transitions.

[0054] 2. The present invention decomposes the global path into inter-floor and intra-floor subtasks through hierarchical task decomposition, significantly reducing the algorithm complexity of cross-layer planning and improving real-time performance.

[0055] 3. The present invention is suitable for cross-layer environments such as lunar exploration, ruins search and rescue, and high-rise inspections, enabling the robot to autonomously traverse complex structures such as stairs and slopes, reducing manual intervention and risks. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Other features, objects and advantages of the present invention will become more apparent upon reading the detailed description of non-limiting embodiments with reference to the following drawings:

[0057] Figure 1 This is a simulation example diagram of the cross-layer environment of the present invention.

[0058] Figure 2 These are example diagrams of the layered diagram of the present invention, (a) is a cutting diagram with an interval of 1m, and (b) is a point cloud quantity statistics diagram.

[0059] Figure 3 These are elevation example maps of the present invention, (a) is the original elevation map, (b) is the elevation map after segmentation of the drivable area, and (c) is the elevation map after expansion of the no-driving area.

[0060] Figure 4 This is an example diagram of multi-layer elevation of the present invention, (a) is the highest layer, (b) is the middle layer, and (c) is the lowest layer.

[0061] Figure 5 These are example diagrams of gateway identification according to the present invention, where (a) is a schematic diagram of a virtual cuboid and (b) is the final gateway point.

[0062] Figure 6 This is a 3D target point diagram set for the present invention.

[0063] Figure 7 This is an example diagram of cross-layer path planning of the present invention.

[0064] Figure 8 It is a schematic flow chart of the working method of the present invention. DETAILED DESCRIPTION

[0065] The present invention will be described in detail below with reference to specific embodiments. The following examples will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those skilled in the art, several changes and improvements can be made without departing from the scope of the present invention. These all fall within the scope of protection of the present invention.

[0066] Example 1

[0067] According to the present invention, a cross-layer path planning method based on a multi-layer elevation map is provided. Figure 8 As shown, including:

[0068] Step S1: Acquire three-dimensional point cloud data of a multi-layer environment, and adaptively layer the three-dimensional point cloud data to obtain the number of layers corresponding to the point cloud group covering all elevation information. Figure 2 As shown, step S1 includes:

[0069] Step S1.1: Segment the point cloud data according to a fixed spacing to obtain N layers of point cloud. For example, if the point cloud is segmented using a plane spacing of 1 meter, a point cloud with a height of N meters (rounded up) is divided into N layers.

[0070] Step S1.2: Count the number of point clouds in each layer and calculate the corresponding average value. The formula is as follows:

[0071]

[0072] Among them, num 平均 is the average value, N is the N-layer point cloud in step S1.1, num i is the number of point clouds in layer i.

[0073] Step S1.3: Under normal circumstances, the number of point clouds on the plane is significantly greater than that in other places. The layer with a point cloud number greater than the average is identified as a plane layer, and the plane layer index number m can be obtained.

[0074] Step S1.4: Based on the lowest value h0 in the z-axis direction of the multi-layer environment, the plane index m, and the plane interval of 1m, the following formula can be used to calculate the elevation value of each plane layer:

[0075] h=h0+m*1

[0076] The elevation values ​​of adjacent plane layers are averaged, and the elevation plane where the average value is located (the z-axis is its normal) can be used as the tangent plane. There are m-1 tangent planes in the m-layer plane layer. Point clouds are allocated according to the tangent planes. Since the point cloud has a certain thickness, in order to ensure the complete acquisition of elevation information, a certain upward and downward extension is performed when allocating the point cloud. Based on the m-1 tangent planes, the simplest point cloud group of m layers that covers all elevation information is finally obtained.

[0077] Step S2: Construct an elevation map for each layer of point cloud group. For a single layer, the point cloud data is distributed to each grid of the grid map. The two-dimensional coordinate corresponding to each grid has a fixed height value. By storing the height value of each grid, a single-layer elevation layer is obtained. The searched two-dimensional path can be naturally mapped to a three-dimensional path according to the corresponding elevation layer. That is, according to the elevation layer information, the elevation value corresponding to each two-dimensional coordinate point on the path needs to be obtained, and the original (x, y) coordinate sequence is converted into a three-dimensional coordinate point set (x, y, z). While retaining the shape characteristics of the original plane path, it is given a height dimension of the real terrain undulation. The final generated three-dimensional path not only accurately reflects the plane direction, but also presents vertical undulations that change with the terrain. The required multi-layer elevation map can be obtained by combining m layers of elevation layers.

[0078] Step S3: Calculate the height difference between the current grid and the adjacent grid to obtain the passability of the current grid, analyze the terrain, and optimize the elevation map.

[0079] Since the end-point cost function in the path search algorithm often guides the path to be as short as possible, it is very easy to plan an unsafe path close to the corner at the corner of the stairs, which is not conducive to the passage of four-legged dogs. Therefore, the elevation map is optimized and terrain analysis is added. The passability of the grid is obtained by calculating the height difference between the current grid and the adjacent grid, and the terrain is analyzed. When the height difference exceeds the vertical passability of the four-legged dog, the grid is divided into an inaccessible area and the area is appropriately expanded to ensure that the path can be searched away from the corner of the stairs, enter the stairs safely, and pass through the safe area in the middle of the stairs as much as possible.

[0080] Step S4: To connect the planned paths of each layer, identify valid points at the intersection of each layer as gateways. During the point cloud allocation process, there are intersection points between maps of different layers, and the intersection points must exist on the cutting plane. For each cutting plane, find an optimal gateway as the connection point between layers. Step S4 includes:

[0081] Step S4.1: Store all point clouds that intersect the section plane as possible gateway groups:.

[0082] Step S4.2: Construct a virtual cuboid at each possible gateway, and store the point cloud of the points 1 meter away from the current point in the positive and negative directions of the z axis with respect to the current point as the reference.

[0083] Step S4.3: Perform preliminary screening. For the actual valued points in the virtual cuboid, calculate the height difference s between the lowest point cloud and the highest point cloud. If s is less than or equal to the vertical travel distance of the four-legged dog, the possible gateway point corresponding to the virtual cuboid is selected as the candidate gateway point.

[0084] Step S4.4: Since there are some outlier point clouds that interfere with the calculation of gateway points, fine screening is required. By calculating the standard deviation to filter out the outliers, the final gateway point group can be screened out.

[0085] Step S4.5: Calculate the coordinate average of the final gateway point group to obtain the gateway point of the tangent plane. m-1 tangent planes will identify m-1 gateway points.

[0086] Step S5: Based on the constructed multi-layer elevation map and accessible gateway points, a cross-layer path search is performed. During a multi-layer path search, the gateway points between the starting point and the end point are determined. The search begins at the starting point within its layer. Once a gateway point is found, the search continues in the next layer until the end point is reached. Finally, the paths are concatenated to create a cross-layer path in 3D space. The total path is then composed of the path from the starting point to the gateway closest to the starting point, the paths between all gateways, and the path from the gateway closest to the end point to the end point.

[0087] In summary, the method of the present invention addresses the problem of path planning in a cross-layer environment. By constructing a multi-layer elevation map and identifying the only gateway between layers, the path planning problem in a complex environment of multi-layer stairs is converted into a multi-segment plane trajectory splicing problem.

[0088] Furthermore, combined with Figure 1 To the attached Figure 7 The cross-layer path planning method based on multi-layer elevation graphs of the present invention is described in detail as follows:

[0089] For Figure 1 The cross-layer environment shown in the figure obtains its point cloud file. Figure 2 (a), use cutting planes with an interval of 1m to cut the point cloud, such as Figure 2 (b) The number of points in each layer is counted and the average is calculated. Layers exceeding the average are identified as plane layers. For this example point cloud, three plane layers are identified. The average height of adjacent plane layers is taken as the cutting surface. There are two cutting surfaces for three plane layers. The final three-layer minimal point cloud cluster that covers all elevation information is allocated based on the cutting surface.

[0090] Calculate the elevation layer of each point cloud group, such as Figure 3 (a) is the elevation layer of the topmost point cloud group, which is used to segment the drivable area by the height difference between adjacent grids, as shown in Figure 3 (b), the black area is the segmented non-driving area. To ensure the safety of the path, the non-driving area is appropriately expanded, as shown in Figure (c). The red area is the final no-driving area. The path search algorithm does not search for possible path points in this area. By combining the elevation maps of the three-layer point cloud group, the final multi-layer elevation map can be obtained, as shown in Figure 5. Figure 4 .

[0091] Next, we identify the gateway, and identify a pass gateway for each cutting surface. Figure 5 (a),For the cutting surfaces of the highest and middle layers, all point clouds intersecting with the cutting surfaces are considered as possible gateway points. Based on each possible gateway point, a virtual cuboid is constructed to store all point clouds 1m away from it in the positive and negative directions of the z-axis. Through preliminary and fine screening, possible gateway points that do not meet the vertical passage capacity of the quadruped dog and outlier gateway points are filtered out, and finally a group of gateway points between layers is obtained. By calculating the average value of their coordinates, the unique gateway point of the cutting surface is obtained. Two gateway points are identified on the two cutting surfaces, such as Figure 5 (b) is shown by the red dots.

[0092] Based on the constructed multi-layer elevation map and gateway points, cross-layer path search is performed. Figure 6 , set the 3D end point containing height information. Figure 7,When the starting point is in the middle layer and the end point is in the top layer, and there is a ,passing gateway in between, the cross-layer path is composed of the ,path 1 from the starting point to the gateway and the path 2 from ,the gateway to the end point.

[0093] To overcome the inefficiency of multi-layer path search for ground robots based on voxel maps, the present invention uses 2.5D elevation maps for path planning in multi-layer 3D environments. To achieve cross-layer path connectivity, the optimal cross-layer path is generated, and gateways between layers are identified as transition points for searching cross-layer paths, thereby achieving inter-layer connectivity.

[0094] Example 2

[0095] The present invention also provides a cross-layer path planning system based on a multi-layer elevation map. The cross-layer path planning system based on a multi-layer elevation map can be implemented by executing the process steps of the cross-layer path planning method based on a multi-layer elevation map, that is, those skilled in the art can understand the cross-layer path planning method based on a multi-layer elevation map as a preferred implementation of the cross-layer path planning system based on a multi-layer elevation map.

[0096] According to the present invention, a cross-layer path planning system based on a multi-layer elevation map is provided, comprising:

[0097] Module M1: Acquires 3D point cloud data from a multi-layer environment and adaptively layers the 3D point cloud data to obtain the number of layers corresponding to the point cloud group covering all elevation information. Module M1 includes: Module M1.1: Splits the point cloud data according to a fixed spacing to obtain N layers of point cloud. Module M1.2: Counts the number of point clouds in each layer and calculates the corresponding average value using the following formula:

[0098]

[0099] Among them, num 平均 is the average value, N is the N-layer point cloud in module M1.1, num i is the number of point clouds in the i-th layer. Module M1.3: Identify the layer with a point cloud number greater than the average as a plane layer, and obtain the plane layer index number m. Module M1.4: Calculate the elevation value of each plane layer based on the lowest value h0 in the z-axis direction of the multi-layer environment, the plane layer index m and the plane interval. The formula is as follows: h = h0 + m*1. Module M1.5: Take the average of the elevation values ​​of adjacent plane layers. The elevation surface where the average value is located can be used as the tangent plane. There are m-1 tangent planes for the m-layer plane layer. The point cloud is allocated according to the tangent plane, and finally the simplest point cloud group of the m-layer that covers all elevation information is obtained.

[0100] Module M2: Constructing an elevation map for each layer of point cloud clusters. In Module M2, the point cloud data of a single layer is assigned to each grid of the raster map. The two-dimensional coordinates corresponding to each grid have a fixed height value. By storing the height values ​​of each grid, a single-layer elevation map is generated. The searched two-dimensional path can be naturally mapped to a three-dimensional path based on the corresponding elevation map. By combining m layers of elevation maps, the desired multi-layer elevation map can be obtained.

[0101] Module M3: Calculates the height difference between the current grid and adjacent grids to determine the current grid's accessibility, analyzes the terrain, and optimizes the elevation map. Module M3 calculates the height difference between the current grid and adjacent grids to determine the current grid's accessibility, analyzes the terrain, and if the height difference exceeds the vertical accessibility of a quadruped dog, the current grid is divided into impassable areas and appropriately expanded.

[0102] Module M4: Identify the valid points at the intersection of each layer as the pass gateway point. The module M4 includes: Module M4.1: Store all point clouds that intersect with the section as possible gateway groups. Module M4.2: Construct a virtual cuboid at each possible gateway, and store the point cloud 1m away from the point in the positive and negative directions of the z-axis based on the current point. Module M4.3: Perform preliminary screening, and calculate the height difference s between the lowest point cloud and the highest point cloud for the actual value in the virtual cuboid. If s is less than or equal to the vertical passage distance of the four-legged dog, the possible gateway point corresponding to the virtual cuboid is used as the alternative gateway point. Module M4.4: Perform fine screening, filter out outliers by calculating the standard deviation, and screen out the final gateway point group. Module M4.5: Calculate the coordinate average of the final gateway point group to obtain the gateway point of the section plane.

[0103] Module M5: Perform cross-layer path search based on the constructed multi-layer elevation map and access gateway points.

[0104] Those skilled in the art will appreciate that, in addition to implementing the system and its various devices, modules, and units provided by the present invention in purely computer-readable program code, it is entirely possible to implement the same functions of the system and its various devices, modules, and units provided by the present invention in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; the devices, modules, and units for implementing various functions can also be considered as both software modules implementing the method and structures within the hardware component.

[0105] The above describes specific embodiments of the present invention. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art may make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. The embodiments of this application and the features in the embodiments may be combined with each other in any manner unless there is a conflict.

Claims

1. A cross-layer path planning method based on a multi-layer elevation map, characterized in that: include: Step S1: Acquire three-dimensional point cloud data of a multi-layer environment, and adaptively layer the three-dimensional point cloud data to obtain the number of layers corresponding to the point cloud group covering all elevation information; Step S2: constructing the elevation map of each layer of point cloud group; Step S3: Calculate the height difference between the current grid and the adjacent grid to obtain the passability of the current grid, analyze the terrain, and optimize the elevation map; Step S4: identifying valid points at the intersection of each layer as gateway points; Step S5: Based on the constructed multi-layer elevation map and the gateway points, a cross-layer path search is performed.

2. The cross-layer path planning method based on a multi-layer elevation map according to claim 1 is characterized in that: The step S1 comprises: Step S1.1: Segment the point cloud data according to a fixed spacing to obtain N layers of point clouds; Step S1.2: Count the number of point clouds in each layer and calculate the corresponding average value. The formula is as follows: Among them, num 平均 is the average value, N is the N-layer point cloud in step S1.1, num i is the number of point clouds in the i-th layer; Step S1.3: Identify the layer with a point cloud number greater than the average as a plane layer, and obtain the plane layer index number m. Step S1.4: Calculate the elevation of each plane layer based on the lowest value h0 in the z-axis direction of the multi-layer environment, the plane layer index m, and the plane interval. The formula is as follows: h=h0+m*1 Step S1.5: Take the average elevation value of the adjacent plane layers and calculate the average value. The elevation surface where the average value is located can be used as the tangent plane. There are m-1 tangent planes in the m-layer plane layer. Point clouds are allocated according to the tangent planes, and finally the simplest point cloud group of m layers that covers all elevation information is obtained.

3. The cross-layer path planning method based on multi-layer elevation graph according to claim 1 is characterized in that: In step S2, the point cloud data of a single layer is distributed to each grid of the grid map. The two-dimensional coordinate corresponding to each grid has a fixed height value. By storing the height value of each grid, a single-layer elevation layer is obtained. The searched two-dimensional path can be naturally mapped to a three-dimensional path according to the corresponding elevation layer. By combining m layers of elevation layers, the required multi-layer elevation map can be obtained.

4. The cross-layer path planning method based on multi-layer elevation graph according to claim 1 is characterized in that: The step S3 includes obtaining the passability of the current grid by calculating the height difference between the current grid and the adjacent grid, analyzing the terrain, and when the height difference exceeds the vertical passability of the four-legged dog, dividing the current grid into inaccessible areas and performing appropriate expansion processing on the inaccessible areas.

5. The cross-layer path planning method based on multi-layer elevation graph according to claim 1 is characterized in that: The step S4 comprises: Step S4.1: store all point clouds intersecting the section plane as possible gateway groups; Step S4.2: Construct a virtual cuboid at each possible gateway, and store the point cloud of the points 1 meter away from the current point in the positive and negative directions of the z axis; Step S4.3: Perform a preliminary screening. For the actual points with values ​​in the virtual cuboid, calculate the height difference s between the lowest point cloud and the highest point cloud. If s is less than or equal to the vertical distance traveled by the four-legged dog, then the possible gateway point corresponding to the virtual cuboid is selected as the candidate gateway point. Step S4.4: Perform fine screening to filter out outliers by calculating the standard deviation and select the final gateway point group. Step S4.5: Calculate the coordinate average of the final gateway point group to obtain the gateway point of the tangent plane.

6. A cross-layer path planning system based on multi-layer elevation maps, characterized in that: include: Module M1: Acquire three-dimensional point cloud data of a multi-layer environment, and adaptively layer the three-dimensional point cloud data to obtain the number of layers corresponding to the point cloud group covering all elevation information; Module M2: Construct elevation map of each layer of point cloud group; Module M3: Calculate the height difference between the current grid and the adjacent grid to obtain the accessibility of the current grid, analyze the terrain, and optimize the elevation map; Module M4: Identify the valid points at the intersection of each layer as the gateway points; Module M5: Perform cross-layer path search based on the constructed multi-layer elevation map and access gateway points.

7. The cross-layer path planning system based on multi-layer elevation maps according to claim 6 is characterized in that: The module M1 includes: Module M1.1: Segment the point cloud data according to a fixed spacing to obtain N layers of point cloud; Module M1.2: Count the number of point clouds in each layer and calculate the corresponding average value. The formula is as follows: Among them, num 平均 is the average value, N is the N-layer point cloud in module M1.1, num i is the number of point clouds in the i-th layer; Module M1.3: Identify the layer with a point cloud number greater than the average as a plane layer, and obtain the plane layer index number m. Module M1.4: Calculate the elevation of each plane layer based on the lowest value h0 in the z-axis direction of the multi-layer environment, the plane layer index m, and the plane interval. The formula is as follows: h=h0+m*1 Module M1.5: Take the average elevation value of adjacent plane layers and use the elevation surface where the average value is located as the tangent plane. There are m-1 tangent planes in m-layer plane layers. Point clouds are allocated according to the tangent planes, and finally the simplest point cloud group covering all elevation information in m layers is obtained.

8. The cross-layer path planning system based on multi-layer elevation graph according to claim 6, characterized in that: In the module M2, the single-layer point cloud data is distributed to each grid of the grid map. The two-dimensional coordinate corresponding to each grid has a fixed height value. By storing the height value of each grid, a single-layer elevation layer is obtained. The searched two-dimensional path can be naturally mapped to a three-dimensional path according to the corresponding elevation layer. By combining the m layers of elevation layers, the required multi-layer elevation map can be obtained.

9. The cross-layer path planning system based on multi-layer elevation graph according to claim 6, characterized in that: The module M3 includes obtaining the passability of the current grid by calculating the height difference between the current grid and the adjacent grid, analyzing the terrain, and when the height difference exceeds the vertical passability of the four-legged dog, dividing the current grid into inaccessible areas and performing appropriate expansion processing on the inaccessible areas.

10. The cross-layer path planning system based on multi-layer elevation graph according to claim 6, characterized in that: The module M4 includes: Module M4.1: Store all point clouds intersecting the section plane as possible gateway groups; Module M4.2: Construct a virtual cuboid at each possible gateway, and store the point cloud of the points 1 meter away from the current point in the positive and negative directions of the z axis. Module M4.3: Perform a preliminary screening. For the actual points with values ​​in the virtual cuboid, calculate the height difference s between the lowest point cloud and the highest point cloud. If s is less than or equal to the vertical distance traveled by the four-legged dog, then the possible gateway point corresponding to the virtual cuboid is selected as the candidate gateway point. Module M4.4: Perform fine screening, filter out outliers by calculating the standard deviation, and select the final gateway point group. Module M4.5: Calculate the coordinate average of the final gateway point group to obtain the gateway point of the tangent plane.

Images