A method for constructing navigation cost map for unmanned vehicles in complex and unknown environments

Abstract

The present invention discloses a method for constructing a navigation cost map for an unmanned vehicle in a complex and unknown environment. The method utilizes real-time point cloud data from a laser radar and a depth camera, and constructs a navigation cost map through steps such as point cloud data preprocessing, segmentation of ground point clouds and non-ground point clouds, edge detection of impassable areas, construction of a 2.5D elevation grid map, and slope analysis. The present invention focuses on solving the problems of point cloud segmentation, edge detection of impassable areas, and terrain slope analysis in the process. The advantage is that it can not only detect obstacles above the ground, but also detect the edges of impassable areas below the ground such as potholes. In addition, the present invention can also detect areas where unmanned vehicles are difficult to pass through through terrain analysis. The method of the present invention can enable unmanned vehicles to autonomously identify obstacles and construct navigation maps in complex and unknown environments in the wild.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned vehicle navigation and obstacle avoidance, and specifically, is a method for constructing a navigation cost map in a complex unknown environment. Background Art

[0002] At present, the environmental exploration and map construction technology of unmanned vehicles in indoor scenes or outdoor structured environments such as flat roads has achieved remarkable results under the research of many scholars. It is still technically difficult to build a comprehensive three-dimensional grid map, so that when facing complex terrain obstacles, it is impossible to accurately judge the safety of the surrounding passable areas, so it is difficult to ensure the safety of unmanned vehicles during travel. How to identify safe passable areas based on the perception sensors of unmanned vehicles and lay the foundation for unmanned vehicles to perform various tasks in the wild is still a research difficulty and focus in this field.

[0003] Patent CN 114035584 A proposes a method for robot to detect obstacles. This method obtains the occupied height of each grid by matching the point cloud information around the robot with the grid map, and marks and stores the obstacle area based on the difference detection between adjacent grids, thereby avoiding the cumulative error caused by absolute interpolation detection and improving the efficiency and stability of obstacle identification. However, this method cannot effectively detect the passable area in the face of obstacles such as pits that cause faults in the point cloud map or detection blind spots. The present invention constructs an unmanned vehicle navigation map for situations in outdoor environments where the ground is undulating and there are both concave and convex obstacles. Summary of the invention

[0004] The purpose of the present invention is to provide a method for constructing a navigation cost map for an unmanned vehicle in a complex and unknown environment, using real-time point cloud data from a laser radar and a depth camera to achieve the construction of an unmanned vehicle navigation map in a field environment where the ground is undulating and there are both concave and convex obstacles.

[0005] The technical solution to achieve the purpose of the present invention is: a method for constructing a navigation cost map for an unmanned vehicle in a complex unknown environment, first using an empty map as a global cost map, then cropping and line-sequencing preprocessing the point cloud data of the lidar, extracting non-ground point clouds and the edge point clouds of potholes in the ground point cloud based on ground segmentation, and jointly calculating the basic obstacle map layer with the point cloud of the depth camera. At the same time, an octree map is constructed based on the point cloud data of the lidar, mapped to a raster map with elevation information, the ground slope and undulation are analyzed, an additional obstacle layer is calculated, and the inaccessible area is marked. The two obstacle layers are superimposed and fused, and a local cost map is obtained by obstacle unit fitting and expansion processing, and finally the local cost map is mapped to the global cost map to construct the final navigation cost map.

[0006] The specific implementation steps are as follows:

[0007] Step (1) creates a blank map as the global cost map, indicating that there is no prior knowledge and the environment is unknown.

[0008] Step (2), subscribe to the point cloud data of the depth camera as a sensor data source for the obstacle map layer in the local cost map.

[0009] Step (3), acquisition and preprocessing of laser radar point cloud data. Subscribe to the laser radar point cloud data, set a sampling area within a certain threshold range around the unmanned vehicle, and remove point clouds outside the area.

[0010] Step (4) performs ground segmentation processing, extracts the non-ground point cloud set and the point cloud located at the edge of the pothole in the ground point cloud set, and constructs an obstacle point cloud map as another sensor data source for the obstacle map layer.

[0011] Step (5), making a 2.5D elevation grid map. Use the original LiDAR point cloud to construct a local octree map within a certain threshold range around the unmanned vehicle, and project the octree map onto a two-dimensional plane to generate a two-dimensional grid map with elevation information.

[0012] Step (6), grid map terrain analysis. Based on the grid map with elevation information, the relative slope value and terrain undulation of each cell are calculated, and the terrain hazard degree is obtained by normalization and weighted summation. Cells with terrain hazard degrees greater than the safety threshold are marked as obstacles, and an additional obstacle layer is generated.

[0013] Step (7) superimposes and fuses the obstacle information in the two obstacle map layers, and uses the Random Sample Consensus (RANSAC) algorithm to fit the cells representing obstacles in the two obstacle map layers into line segments according to their distribution, so as to reduce the computational time of collision detection and improve map stability.

[0014] Step (8) creates an expansion layer in the local cost map. Based on the size of the unmanned vehicle, set the appropriate expansion radius parameter r and scale factor μ. Based on the obstacle map layer, propagate the cost of each obstacle cell to the surrounding cells. The larger the distance value, the smaller the cost value, so as to avoid the unmanned vehicle from approaching obstacles.

[0015] Step (9), superimpose the map layers of the local cost map, project the local cost map to the corresponding position in the global cost map based on the current position information of the unmanned vehicle, and construct a complete navigation cost map.

[0016] Compared with the prior art, the present invention has the following significant advantages: (1) By judging the ground and non-ground point clouds based on angle differential, point cloud segmentation and extraction of pothole edges are achieved, which reduces point cloud misprocessing and improves the comprehensiveness of the unmanned vehicle's environmental perception ability; (2) Based on the octree map, a two-dimensional grid map with elevation information is constructed, which improves the unmanned vehicle's ability to calculate and analyze the slope of the surrounding environment, and helps to plan a safer route. The present invention makes the construction of the cost map more comprehensive and accurate, and realizes the safe navigation of the unmanned vehicle in unstructured scenes in the wild. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a flow chart of a method for constructing a navigation cost map for an unmanned vehicle in a complex unknown environment according to the present invention.

[0018] Figure 2 It is a schematic diagram of a moving window for calculating the slope of a sampling point in the present invention.

[0019] Figure 3 It is a terrain model containing obstacles such as potholes in gazebo simulation in an embodiment of the present invention.

[0020] Figure 4 It is the original point cloud of the laser radar in the embodiment of the present invention.

[0021] Figure 5 This is an example of a non-ground point cloud obtained by ground segmentation processing of a lidar point cloud in an embodiment of the present invention.

[0022] Figure 6 This is an example of the result of extracting the edge point cloud of a pothole in the ground point cloud in an embodiment of the present invention.

[0023] Figure 7 It is a local cost map constructed based on obstacle information in an embodiment of the present invention.

[0024] Figure 8 It is an octree map constructed in an embodiment of the present invention.

[0025] Fig. 9 It is an elevation grid map constructed based on the octree map in an embodiment of the present invention.

[0026] FIG. 10 is a local cost map constructed based on terrain information in an embodiment of the present invention.

[0027] FIG. 11 is a local cost map constructed by integrating obstacle information and terrain information in an embodiment of the present invention. DETAILED DESCRIPTION

[0028] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0029] like Figure 1 As shown, the present invention is a method for constructing a navigation cost map for an unmanned vehicle in a complex unknown environment, and the specific implementation steps are as follows:

[0030] Step 1: Generate a large enough blank map through GIMP and set the resolution to 0.05 meters per pixel. Since there is no obstacle information in the blank map, it can be used as the initial global map in the location environment.

[0031] Step 2: Subscribe to the point cloud data of the depth camera, and after downsampling, use it as a sensor data source for the obstacle map layer in the local cost map.

[0032] Step 3: Acquisition and preprocessing of LiDAR point cloud data. Subscribe to the LiDAR point cloud data, set a sampling area within a certain threshold range around the unmanned vehicle, and remove point clouds outside the area. This step specifically includes:

[0033] Step 3.1: Point cloud clipping. Subscribe to the point cloud data of the lidar, first convert the original point cloud from sensor_msgs::PointCloud2 type to pcl::PointCloud <pcl::pointxyzi>Type, so as to obtain the coordinate information of each point cloud. Then the point cloud is clipped in the height direction, and the height threshold H is set according to the height h of the laser radar itself and the installation height H max =1.2×(h+H), and the point clouds with heights higher than this threshold are trimmed. Secondly, due to the installation position of the laser radar and the size of the unmanned vehicle, the unmanned vehicle itself may reflect part of the laser and interfere with the subsequent segmentation. It is necessary to filter the point clouds at close range, calculate the horizontal distance d of each point cloud relative to the center of the radar, and set the lower limit of the distance threshold R min The upper limit of the distance threshold R max , remove the point cloud outside this threshold range;

[0034] Step 3.2: Point cloud line sequence processing. Store the point cloud according to the horizontal angle resolution of the laser radar, and classify the point clouds with the same horizontal angle (i.e., the point clouds located on the same ray on the XY plane) into the same sequence, and finally store them as a two-dimensional array. First, traverse each point cloud and use formulas (1) and (2) to calculate its horizontal distance d to the center of the laser radar. i and the horizontal azimuth angle θ i , that is, the angle relative to the forward direction of the unmanned vehicle (X-axis direction).

[0035]

[0036]

[0037] Then, all point clouds are classified according to their horizontal azimuth angles, and point clouds with the same angle are classified into the same sequence. Based on the horizontal angle resolution α of the laser radar itself, the total number of sequences (rays) can be obtained according to formula (3). Finally, the point clouds in each sequence are sorted from small to large according to the horizontal distance, thus completing the line sequence of the point cloud.

[0038]

[0039] Step 4: Perform ground segmentation processing, extract the non-ground point cloud set and the point cloud located at the edge of the pothole in the ground point cloud set, and build an obstacle point cloud map. This step specifically includes:

[0040] Step 4.1: Point cloud ground segmentation processing. For point clouds in the same sequence at the same horizontal angle, the height difference between the front and rear points is calculated and used as the main basis for classifying the point cloud as ground or non-ground. The installation height of the laser radar is known to be H, the horizontal distance threshold D is set, and the global expected height threshold H is calculated according to formulas (4) and (5). g and the local expected height threshold H l ,

[0041] H g =-H±tanβ×d i_real (4)

[0042] H l ×h i_prev_real ±tanγ×Δd i_real (5) Where β represents the slope angle threshold of the entire ground, γ represents the slope angle threshold between two adjacent points in the same sequence, and d i_real Indicates the actual horizontal distance from the current point cloud to the laser radar, Δd i_real Indicates the horizontal distance between the current point cloud and the previous point cloud, Δd i_real =d i_real -d i_prev_real ,h i_prev_real Represents the height of the previous point cloud. First, based on the height of the previous point cloud, determine whether the height fluctuation of the current point cloud is within the local expected height threshold. If the current height change is within the threshold and the previous point cloud is a ground point cloud, the current point cloud is also a ground point cloud. If the previous point cloud is a non-ground point cloud, use the global expected height threshold to judge again. If the height change of the current point cloud relative to the previous point cloud is greater than the local height threshold, then determine whether the horizontal distance between the current point cloud and the previous point cloud is greater than the distance threshold D. If the distance between the two points is greater than the threshold, use the global expected height threshold to judge again. In this way, the point cloud is divided into a ground point cloud set P. n and the non-ground point cloud set P ng .

[0043] Step 4.2: Extraction of point cloud of pothole edge. Due to the concave characteristics of potholes, point clouds are usually only distributed on the back half of the pothole. In order to ensure the safety of unmanned vehicles, it is necessary to identify the edge of the front half of the pothole. The specific method is to first create a new point cloud set P ug , used to store the point cloud of the pothole edge. When the current point is detected as a non-ground point cloud and the previous point cloud is a ground point cloud, if the current point cloud height is lower than the global minimum height threshold, the coordinate information of the previous point cloud of the current point cloud is extracted, and two new point clouds are created at the same horizontal position, with heights of vehicle height H and heights of vehicle height H. robot and half of the vehicle height H robot / 2 and store it in the point cloud set P ug In order to facilitate the subsequent processing as an obstacle in the cost map.

[0044] Step 5: Create a 2.5D elevation grid map. Use the original LiDAR point cloud to build a local octree map within a certain threshold range around the unmanned vehicle, and project the octree map onto a two-dimensional plane to generate a two-dimensional grid map with elevation information. This step specifically includes:

[0045] Step 5.1: Construct a local octree map. After obtaining the original point cloud data of the lidar, an incremental octree map is constructed within a rectangular range of 2L with the current position of the unmanned vehicle as the center, and it is dynamically updated as the unmanned vehicle moves. At the same time, in order to reduce the data burden and the amount of calculation, when the horizontal distance d of the unmanned vehicle relative to the starting point satisfies d+l=L, the octree map is cleared, and the octree map is regenerated around the current position as the starting point to reduce the map maintenance cost, where l represents half of the length of the local cost map, and satisfies l<L, ensuring that the exploration range of the octree map is larger than the local cost map, so that the global path planned based on elevation information and distance information can have a certain degree of foresight.

[0046] Step 5.2: Construct an elevation grid map. Project the local octree map onto a two-dimensional plane to construct a two-dimensional grid map with elevation information, that is, the cost value of each grid represents the maximum height value of the voxel at that location, and is updated as the local octree map is updated.

[0047] Step 6: Terrain analysis and application. Create a map layer plug-in for the local cost map, and subscribe to the grid map data obtained in the previous step in the plug-in. Extract and retain the grid map data around the unmanned vehicle within the same size range as the local cost map. According to formula (6), the elevation information index value n corresponding to the specified position on the grid map can be obtained.

[0048] n=floor(p y / r)×w+floor(p x / r) (6) In the formula, floor() means rounding down, (p x ,p y ) represents a certain position in the map relative to the unmanned vehicle, in meters, and r represents the resolution of the grid map. Then a two-dimensional array is used to store the elevation information of the grid, which is convenient for calculating the slope of the adjacent position. For the slope θ of a unit in the grid map s , calculated according to formula (7),

[0049]

[0050] In the formula, f x With f y Respectively represent the elevation change rate of the current unit in the east-west direction and the north-south direction, and are expressed as follows Figure 4 The 3×3 sliding window shown is calculated according to the third-order inverse distance square weighted difference model shown in formula (8):

[0051]

[0052] In the formula, h i Represents the elevation information of the corresponding unit. If there is an unexplored unit in the sliding window, that is, the elevation information is -1, the elevation information of the current sampling unit is used instead. The undulation R is identified by the sum of the absolute values ​​of the elevation differences between the current unit and all units in the current window, as shown in formula (9).

[0053]

[0054] Finally, considering the slope θ s and the ground relief R, the terrain of the current grid cell is evaluated according to formula (10).

[0055]

[0056] In the formula, D represents the judgment result of the terrain danger degree. When D = 0, it means that the road surface is flat and passable, and the slope cost value of this type of grid cell is set to 0; when D = 1, it means that the ground is slightly undulating but within the acceptable range of the unmanned vehicle's passability. The cost value p of this type of grid cell is set according to formula (11).

[0057]

[0058] Where t represents the cost coefficient; when D ≥ 2, it means that the ground is too steep and the driverless car may not be able to pass safely, so this type of grid is marked as an impassable area. round(x) means rounding the value to the nearest integer, α1 and α2 are weight coefficients and α1+α2×1. S th and R th They are the maximum thresholds for slope and undulation, respectively, to achieve normalization of slope and undulation. Finally, the obstacle information in the area is loaded into the additional obstacle layer map layer, a plug-in descriptor file costmap_plugins.xml is created to export this plug-in, and the map layer is called in the navigation configuration file.

[0059] Step 7: Obstacle cell fitting processing. The original cost map is composed of cells in the grid map, where obstacles are represented as occupied cells in the grid map. However, if collision detection is performed on each cell marked as occupied, the calculation will be large and time-consuming, so a clustering method is selected to convert the continuously arranged cells into a single line segment representation. First, the obstacle information in the two obstacle map layers is superimposed and fused, and then the random sampling consensus algorithm (RANSAC) is used to fit the obstacle cells in the same distribution in the grid map into a single line segment according to the data distribution.

[0060] Step 8: Add an expansion layer. Based on the size of the unmanned vehicle, set the appropriate expansion radius parameter λ and the proportional factor μ. On the basis of the obstacle layer, the cost can be propagated from each occupied cell to the surrounding cells. The larger the distance value dis, the smaller the cost value C, so as to avoid the unmanned vehicle from approaching obstacles. The calculation formula of the expansion cost C is as follows:

[0061] C=(254-1)e -1*μ*(dis-λ) (9)

[0062] Step 9: Combine the obtained local cost map with the position information of the unmanned vehicle itself and project it to the corresponding position of the global cost map to finally complete the construction of the unmanned vehicle navigation map.

[0063] Example

[0064] In order to illustrate the effectiveness of the algorithm of the present invention and fully demonstrate that the method has the function of constructing a navigation cost map in a wild mountain environment, the following experiments are completed:

[0065] (1) Experimental initial conditions and parameter settings

[0066] In the simulation experiment, the unmanned vehicle is equipped with a depth camera, IMU and 32-line laser radar. A mountain environment with steep slopes, potholes and other impassable areas is constructed in gazebo, and the unmanned vehicle is navigated to different obstacle spaces, and a navigation map is constructed based on the surrounding terrain.

[0067] (2) Experimental results analysis

[0068] Attached Figure 3 The gazebo simulation environment and the location of the unmanned vehicle. The simulation environment has concave obstacles and convex obstacles. Figure 4 The original lidar point cloud map obtained by the unmanned vehicle, attached Figure 5 It is the non-ground point cloud map after ground segmentation. Figure 6 With attached Figure 7 They are the recognition of the pothole edge point cloud and the local cost map constructed based on obstacle information. Figure 8 With attached Fig. 9 is the octree map and the elevation grid map generated by projecting it onto a two-dimensional plane. Attached Figure 10 is a local cost map constructed based on terrain information. The parameters α1 and α2 in formula (10) are set to 0.7 and 0.3 respectively. The slope threshold S th Set to 0.65, the fluctuation threshold R th Set to 1.75. Figure 11 is a local cost map constructed by integrating obstacle information and slope information. From the simulation diagram, it can be seen that this method can effectively complete the perception and construction of the surrounding environment and can adapt to various field terrain obstacles.< / pcl::pointxyzi>

Claims

1. A method for constructing a navigation cost map for an unmanned vehicle in a complex unknown environment, characterized by: First, an empty map is used as the global cost map. Secondly, the point cloud data of the lidar is cropped and preprocessed into lines. Based on ground segmentation, the non-ground point cloud and the edge point cloud of potholes in the ground point cloud are extracted, and the basic obstacle map layer is calculated together with the point cloud of the depth camera. At the same time, an octree map is constructed based on the point cloud data of the lidar and mapped to a raster map with elevation information. The ground slope and undulation are analyzed, and an additional obstacle layer is calculated to mark the inaccessible area. The two obstacle layers are superimposed and fused, and the local cost map is obtained by obstacle unit fitting and expansion processing. Finally, the local cost map is mapped to the global cost map to construct the final navigation cost map.

2. The method according to claim 1, characterized in that: The specific implementation steps are as follows: Step (1), create a blank map as the global cost map, indicating that there is no prior knowledge and the environment is unknown; Step (2), subscribing to the point cloud data of the depth camera as a sensor data source for the obstacle map layer in the local cost map; Step (3), obtaining laser radar point cloud data, and performing cropping and line sequence preprocessing; subscribing to the laser radar point cloud data, setting a sampling area within a threshold range around the unmanned vehicle, and removing point clouds outside the area; Step (4), performing ground segmentation processing, extracting the non-ground point cloud set and the point cloud located at the edge of the pothole from the ground point cloud set, and constructing an obstacle point cloud map as another sensor data source for the obstacle map layer; Step (5), making a 2.5D elevation grid map; Use the original LiDAR point cloud to construct a local octree map within the threshold range around the unmanned vehicle, and project the octree map onto a two-dimensional plane to generate a two-dimensional raster map with elevation information. Step (6), grid map terrain analysis; Based on the grid map with elevation information, the relative slope value and terrain relief of each unit are calculated, and the terrain hazard degree is obtained by normalization and weighted summation. The units with terrain hazard degree greater than the safety threshold are marked as obstacles, and an additional obstacle layer is generated. Step (7), superimposing and fusing the obstacle information in the two obstacle map layers, using a random sampling consensus algorithm to fit the cells representing obstacles in the two obstacle map layers into line segments according to their distribution; Step (8), making an expansion layer in the local cost map; Based on the size of the unmanned vehicle, the expansion radius parameter r and the scale factor μ are set, and based on the obstacle map layer, the cost of each obstacle cell is propagated to the surrounding cells; Step (9), superimpose the map layers of the local cost map; based on the current position information of the unmanned vehicle, project the local cost map to the corresponding position in the global cost map to construct a complete navigation cost map.

3. The method according to claim 2, characterized in that The implementation method of step 3 is as follows: Step 3.1: Point cloud clipping processing; subscribe to the point cloud data of the lidar, first convert the original point cloud from sensor_msgs::PointCloud2 type to pcl::PointCloud <pcl::pointxyzi>Type, so as to obtain the coordinate information of each point cloud; then the point cloud is clipped in the height direction, and the height threshold H is set according to the height h of the laser radar itself and the installation height H max =1.2×(h+H), and cut off the point cloud with a height higher than this threshold; secondly, set the distance threshold R min With R max , remove the point cloud outside this threshold range;< / pcl::pointxyzi> Step 3.2: Point cloud line sequence processing; store point clouds according to the horizontal angle resolution of the laser radar, and classify point clouds with the same horizontal angle into the same sequence. Point clouds with the same horizontal angle are point clouds located on the same ray on the XY plane, and are finally stored as a two-dimensional array; first traverse each point cloud and use formulas (1) and (2) to calculate its horizontal distance d to the center of the laser radar respectively. i and the horizontal azimuth angle θ i , that is, the angle relative to the X-axis direction of the unmanned vehicle's forward direction Then, all point clouds are classified according to their horizontal azimuth angles, and point clouds with the same angle are classified into the same sequence. Based on the horizontal angle resolution α of the laser radar itself, the total number of sequence rays can be obtained according to formula (3). Finally, the point clouds in each sequence are sorted from small to large according to the horizontal distance, thus completing the line sequence of the point cloud.

4. The method according to claim 2, characterized in that: The implementation method of step 4 is as follows: Step 4.1: Point cloud ground segmentation processing; For point clouds in the same sequence at the same horizontal angle, the height difference between the front and rear points is calculated, and this is used as the main judgment basis to classify the point cloud as ground or non-ground; the installation height of the laser radar is known to be H, the horizontal distance threshold D is set, and the global expected height threshold H is calculated according to formulas (4) and (5) respectively g and the local expected height threshold H l , H g =-H±tanβ×d i_real (4) H l =h i_prev_real ±tanγ×Δd i_real (5) Where β represents the slope angle threshold of the entire ground, γ represents the slope angle threshold between two adjacent points in the same sequence, and d i_real Indicates the actual horizontal distance from the current point cloud to the laser radar, Δd i_real Indicates the horizontal distance between the current point cloud and the previous point cloud, Δd i_real =d i_real -d i_prev_real ,h i_prev_real Indicates the height of the previous point cloud; first, based on the height of the previous point cloud, determine whether the height fluctuation of the current point cloud is within the local expected height threshold range. If the current height change is within the threshold range and the previous point cloud is a ground point cloud, the current point cloud is also a ground point cloud. If the previous point cloud is a non-ground point cloud, use the global expected height threshold to determine again; if the height change of the current point cloud relative to the previous point cloud is greater than the local height threshold, then determine whether the horizontal distance between the current point cloud and the previous point cloud is greater than the distance threshold D. If the distance between the two points is greater than the threshold, use the global expected height threshold to determine again; in this way, the point cloud is divided into a ground point cloud set P. n and the non-ground point cloud set P ng ; Step 4.2: Extract the point cloud of the pothole edge; first create a new point cloud set P ug , used to store the point cloud of the pothole edge; when the current point is detected as a non-ground point cloud and the previous point cloud is a ground point cloud, if the current point cloud height is lower than the global minimum height threshold, the coordinate information of the previous point cloud of the current point cloud is extracted, and two new point clouds are created at the same horizontal position, with heights of vehicle body height H and heights of vehicle body height H. robot and half of the vehicle height H robot / 2 and store it in the point cloud set P ug middle.

5. The method according to claim 2, characterized in that: The implementation method of step 5 is as follows: Step 5.1: construct a local octree map; after obtaining the original point cloud data of the laser radar, construct an incremental octree map within a rectangular range of 2L with the current position of the unmanned vehicle as the center, and dynamically update it as the unmanned vehicle moves; when the horizontal distance d traveled by the unmanned vehicle relative to the starting point satisfies d+l=L, clear the octree map, and regenerate the octree map around the current position as the starting point, where l represents half of the length of the local cost map, and satisfies l<L, to ensure that the exploration range of the octree map is larger than the local cost map; Step 5.2: Construct an elevation grid map; project the local octree map onto a two-dimensional plane to construct a two-dimensional grid map with elevation information, that is, the cost value of each grid represents the maximum height value of the voxel at that location, and is updated as the local octree map is updated.

Images