A method for constructing a terrain passability cost map based on cosine soft threshold mapping and probability fusion

Abstract

This invention relates to a method for constructing a terrain navigability cost map based on cosine soft thresholding and probabilistic fusion, belonging to the field of robot environmental perception and map building technology. It includes elevation grid construction, step height feature extraction, local slope feature extraction, roughness feature extraction, cosine soft thresholding, probabilistic OR fusion, visual semantic-assisted correction, and cost map generation. Advantages include: cosine soft thresholding allows the cost map to reflect terrain difficulty in a gradient manner; probabilistic OR fusion enables multi-dimensional safety-priority fusion; semantic-geometric joint correction reduces the terrain misclassification rate from approximately 18% to below 5%; the parameterized design of six physical parameters adapts to different robot platforms; the output is compatible with the ROS costmap_2d interface; and zero-derivative boundary conditions eliminate cost jumps in critical regions.

Description

Technical Field

[0001] This invention relates to the field of robot environmental perception and mapping technology, and particularly to a method for converting 3D point cloud data into a gradient-based drivability cost map. Based on the physical motion capability parameters of a mobile robot, this method utilizes two core technologies—cosine soft thresholding and probabilistic OR fusion—to extend the traditional binary cost map into a cost map that reflects the continuous gradient of terrain drivability difficulty. This method is suitable for navigation tasks of legged robots (including quadrupedal robotic dogs and bipedal humanoid robots), tracked robots, and hybrid wheel-legged robots in urban sidewalks, staircases, ruins, construction sites, and unstructured terrain in the wild. Background Technology

[0002] The cost map is the core input for mobile robot path planning. Its function is to discretize the environmental space into a two-dimensional grid and assign a numerical value representing the cost of passage to each grid. Path planning algorithms (A*, D*, RRT*) search for low-cost paths from the starting point to the destination based on the numerical information in the cost map. The quality of the cost map directly determines the rationality and effectiveness of the path planning.

[0003] The cost map generation methods widely used in industry today, typified by the costmap_2d module in the ROS Navigation Stack, employ a binary judgment mechanism based on a height threshold. This mechanism sets a fixed height threshold (typically 5 to 10 centimeters), marking regions in the 3D point cloud with a height exceeding this threshold as having a cost value of 254 (fatal obstacle), and regions with a height below the threshold as having a cost value of 0 (free passage). This binarization method suffers from the following four structural defects:

[0004] First, the binarization threshold is too conservative for robots with legged locomotion capabilities. Quadrupedal robot dogs can cross steps 15 to 25 centimeters high and walk stably on slopes with an inclination angle of 25 to 35 degrees, but the binarization method marks all these areas as fatal obstacles, resulting in about 40% of the passable areas being mislabeled.

[0005] Second, the binarized cost map loses the gradient information of the terrain and cannot distinguish between "completely flat ground" and "steps that are passable but have varying degrees of difficulty in passage".

[0006] Third, traditional methods lack parameterized modeling of the differences in motion capabilities among different robot platforms and adopt a uniform fixed height threshold.

[0007] Fourth, when relying solely on point cloud geometric analysis for terrain assessment, the edges of walls, the bottom of guardrails, and the chassis of parked vehicles may resemble steps or slopes in terms of point cloud geometric features, lacking semantic-level auxiliary assessment capabilities.

[0008] In summary, existing cost map generation methods have four key shortcomings: (i) binarization judgment cannot express the continuous gradient of terrain traversal difficulty; (ii) lack of parametric modeling based on robot physical motion capabilities; (iii) lack of a joint judgment mechanism for semantic and geometric information; and (iv) the output cost map cannot provide refined terrain difficulty information for downstream path planning algorithms. Summary of the Invention

[0009] This invention provides a method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion. It addresses the shortcomings of the existing technology mentioned above and solves the problems that traditional binary cost maps cannot express terrain difficulty gradients, lack parameterized modeling of robot physical capabilities, lack semantic-geometric joint judgment mechanism, and cannot provide fine terrain information to downstream applications.

[0010] The technical solution adopted by this invention includes the following steps:

[0011] Step 1: Elevation grid construction;

[0012] Step 2: Extraction of step height features;

[0013] Step 3: Local slope feature extraction: For each effective grid cell Take it as the center The average elevation data of the valid grid cells within the window; for grid cells located at the elevation grid boundary, The window only contains the index in The actual and valid grid cells within the range; when the number of valid grid cells within the window is less than 3, the slope of that grid cell. Set to 0 degrees; when the number of valid grid cells within the window... When the least squares method is used to fit the plane, the specific method is as follows: using the center coordinates of each effective grid cell within the window ( ) is the independent variable and the corresponding average elevation. Given the dependent variable, find the solution that makes the sum of squared residuals equal to... Plane parameters that achieve minimum value If ), then the normal vector of the fitting plane is , here This represents the normal vector of the fitted plane, with the subscript f indicating "fitted". The formula for calculating the local slope angle is:

[0014]

[0015] in For the normal vector in The absolute value of the directional component. Let be the Euclidean norm of the normal vector;

[0016] Step 4: Roughness Feature Extraction: For each effective mesh cell... Calculate the root mean square deviation of all point clouds falling into the grid from the fitted plane in step three. When the mesh fails to fit the plane in step three because it has fewer than three effective neighbors, Set to 0; when the fitted plane exists, The calculation formula is:

[0017]

[0018] in ) is a point To fit the plane in step three The vertical distance is calculated using the following formula:

[0019]

[0020] in The parameters are the plane parameters obtained by least squares fitting in step three.

[0021] Step 5, Cosine Soft Threshold Mapping: Based on the mobile robot's physical mobility capabilities, three sets of physical capability parameters are pre-set: step height parameter pair ( ), slope parameter pair ( ) and roughness parameter pair ( );in This is the upper limit of the step height for robots to pass through without any cost. This represents the maximum step height that the robot can cross. The maximum slope is without cost. The maximum passable slope; The upper limit of roughness without cost. For the maximum passable roughness; for each effective mesh cell Calculate the three sub-weights separately:

[0022] Step height sub-weight Calculation of )

[0023]

[0024]

[0025]

[0026] Slope sub-weights ) and roughness sub-weights ) Adopted with The exact same cosine mapping function structure, respectively with ( )and( () as parameter pairs;

[0027] The cosine soft threshold mapping function at x = x free The function value is 0 and the first derivative dw / dx = 0 at x = x max The function value is 1 and the first derivative dw / dx = 0; the physical meaning of the zero derivative boundary condition is: the weight curve smoothly enters and exits at the two endpoints of "completely passable" and "completely impassable", with a slope of zero, to avoid cost abrupt changes at the threshold boundary;

[0028] Step Six, Probability or Fusion: Combine the three sub-weights calculated in Step Five. , , Integrate into a comprehensive accessibility weight The fusion formula is:

[0029]

[0030] in The first one calculated in step five Sub-weights of topographic features, Take the step height in sequence ,slope and roughness This fusion strategy ensures that: when any one of the sub-weights is close to 1, the overall weight increases significantly; and the overall weight is zero if and only if all three sub-weights are zero simultaneously.

[0031] Step 7, Visual Semantic Assisted Correction: Run a semantic segmentation network on an image stream synchronized with the 3D point cloud in time, classifying ground regions in the image into four semantic categories: "steps," "slopes," "flat ground," and "obstacles." Backproject the pixel-level semantic labels of the image onto an elevation grid, and apply the comprehensive weights calculated in Step 6. Make corrections, and the correction rules are as follows:

[0032]

[0033]

[0034]

[0035]

[0036] in The upper limit parameter for step cost is 0.7, with a value range of 0.5 to 0.8. The fourth rule applies to pixels with a semantic segmentation confidence of less than 0.6, which are marked as "uncertain", as well as mesh cells to which no pixels are mapped due to view occlusion or missing point clouds. In this case, the results of the geometric analysis in steps two to six are completely relied upon.

[0037] Step 8, Cost Map Generation: The adjusted comprehensive weights from Step 7 are used to generate the cost map. Mapped to the integer cost space according to the following linear mapping relationship. :

[0038]

[0039] in This indicates that passage is completely free. This indicates a fatal obstacle that prevents passage; for grid cells marked as unobserved areas in step one, a cost of 127 and a cost space are assigned. The intermediate value represents the unknown area; thus, a traversability cost map covering the entire elevation grid is generated.

[0040] Step one of the present invention specifically includes:

[0041] Receive 3D point cloud data output from a 3D perception system, wherein the 3D point cloud data must be represented in a gravity-aligned world coordinate system, wherein The axis points towards the zenith, and the horizontal plane is defined as... Coordinate isosurfaces; the three-dimensional point cloud is projected onto a two-dimensional uniform grid on a horizontal plane to construct a 2.5D elevation grid; the grid resolution is set to... Unit: meters. The origin coordinates of the elevation grid are defined as follows:

[0042] Total number of grid columns Total number of grid rows ,in ; The grid row index is denoted as ), column index is denoted as Each grid cell is denoted as For each three-dimensional point The grid cell to which it belongs is determined by the following formula: When the coordinates of a point fall exactly on the grid boundary line, according to The mathematical definition of the function belongs to the grid cell with the smaller index; for each grid cell... Statistical analysis of all point clouds falling within this grid area Coordinate values, calculate maximum elevation Minimum elevation and average elevation ,in To fall into the grid cell The number of point clouds; for The grid cells are marked as unobserved areas, i.e., non-effective grid cells.

[0043] Step two of this invention specifically includes:

[0044] For each valid grid cell , For each grid cell, calculate the maximum height abrupt change between it and its 8-neighbor grid cells. The 8-neighborhood is defined as a grid. The top ,Down ,Left ,right and top left Top right Bottom left Bottom right There are 8 adjacent grids in total. The row index of the neighboring grid is denoted as... Column index is denoted as For grid cells located at the elevation grid boundary, or ,or or Its 8-neighbor set Only include indexes in If there are fewer than 8 neighboring grids actually existing within the range, they are directly ignored; in all formulas of this method, , This indicates the row and column index of the current grid. , The row and column indices represent the neighborhood grid, and the meanings of the four indices remain consistent throughout the text; the formula for calculating the step height is:

[0045]

[0046] in For grid The 8-neighbor set of the boundary grid has fewer than 8 elements. For the current grid Average elevation value, Neighborhood grid The average elevation value.

[0047] The unified mathematical expression for the cosine soft threshold mapping function in step five of this invention is:

[0048]

[0049]

[0050]

[0051] in For topographic features, The threshold is costless. The maximum passable threshold; in the transition zone The inner function value is strictly monotonically increasing, and at the midpoint of the interval... The maximum slope is obtained at this point. )).

[0052] The mobile robots described in this invention include legged robots such as quadrupedal robot dogs and bipedal humanoid robots, as well as tracked robots and wheel-leg hybrid robots; for different robot platforms, three sets of physical capability parameters are adjusted ( ), ( ), ( This allows for adaptation to the differences in motion capabilities across different platforms without requiring modifications to the algorithm itself.

[0053] The probability or fusion strategy in step six of this invention satisfies the following three mathematical properties: 1) When hour, ;2) When , , When any one of them equals 1, 3) When all three sub-weights are between 0 and 1, Strictly greater than ).

[0054] The specific configuration of the semantic segmentation network in step seven of this invention is as follows: the network architecture adopts BiSeNet-V2, and the input resolution is... The network was trained using a self-built dataset containing at least 5000 labeled images. These images were collected from three scenarios: urban sidewalks, indoor staircases, and construction sites, all captured using a monocular camera under natural lighting conditions. LabelMe was used for labeling, employing pixel-level semantic labels. The four label categories were defined as follows: "Steps" were ground edges with height abrupt changes exceeding 3 cm; "Ramps" were slopes with a gradient exceeding 5 degrees; "Flat Ground" was areas with a gradient less than 5 degrees and height abrupt changes less than 3 cm; and "Obstacles" included walls, railings, vehicles, and fixed man-made structures. The training framework was PyTorch, and the optimizer was SGD (learning rate 0.01, momentum 0.9, weight decay). The network is trained for 200 epochs, with data augmentation including random cropping, random horizontal flipping, and color jittering. The network output is a pixel-wise class probability map, with pixels having a confidence level below 0.6 marked as "uncertain". Alternatively, it can be pre-trained on the Cityscapes public dataset and then fine-tuned on self-built data.

[0055] In step seven of the present invention The parameter selection method is as follows: In a test scenario containing known steps, for A grid search is performed from 0.4 to 0.9 with a step size of 0.05, using weighted scores. To optimize the objective, select the option that maximizes the score. The values ​​are used as default parameters; the “staircase passage success rate” is defined as the percentage of times a planned path is successfully navigated on a known passable staircase out of the total number of tests; the “obstacle crossing error rate” is defined as the percentage of times a planned path crosses a known impassable obstacle out of the total number of tests; the selection of weights 0.7 and 0.3 reflects the design principle that safety takes precedence over passage efficiency.

[0056] The specific calculation process of back-projecting the semantic tags to the elevation grid in step seven of this invention is as follows: Let the image pixel coordinates be... The camera intrinsic parameter matrix is The camera's extrinsic parameters in the world coordinate system are rotation matrices. Translation vector Back projection consists of three steps: The first step is to convert pixel coordinates into normalized camera coordinates. The second step is to determine the position of the camera's optical center. As the starting point of the ray, with Using the ray direction, a spatial index is created for the 3D point cloud using a KD-Tree, with steps along the ray direction. Uniform sampling is performed, with a maximum search depth of For each sampling point, query the nearest neighbor point cloud in the KD-Tree, and select the point whose distance from the ray is less than 10 ... The nearest point cloud points are used as the back projection result If in If no point cloud point meeting the conditions is found within the range, then that pixel will not participate in semantic correction; the third step is to... Mapping to elevation grid index: When the same grid cell When semantic labels of multiple pixels are received, the final semantic category is determined by majority voting. When two or more categories have the same number of votes, the category with higher cost is selected according to the principle of safety priority, with priority from high to low as follows: obstacle > step > ramp > flat ground.

[0057] The method of this invention also includes a sliding window point cloud map update step: maintaining a point cloud map centered on the robot's current position with a radius of... Local point cloud map ( The default value is 50 meters. When the robot's movement causes the map center to shift by more than 50 meters, the robot will move to a position that is more than 50 meters away from the center of the map. When the value reaches 20%, discard the old point cloud data that exceeds the range of the new window, and integrate the newly acquired point cloud data into the current map after pose transformation. Then, re-execute steps one to eight on the updated complete local point cloud map to generate the updated cost map.

[0058] The beneficial effects of this invention are as follows:

[0059] Cosine soft thresholding enables the cost map to reflect terrain difficulty in a gradient manner. The cosine soft thresholding function continuously maps terrain features to [the appropriate threshold] using the robot's physical motion capability parameters as boundaries. The dimensionless weight space is used. The cosine function is chosen because it naturally satisfies the condition that its first derivative is zero at both boundaries, requiring no additional truncation or normalization. Probabilistic OR fusion achieves multi-dimensional safety-first fusion, treating the sub-weights of the three dimensions as probabilities of independent events and using probabilistic OR for fusion, thus possessing a "safety-first" characteristic. Deterministic correction rules are formulated for the four ground semantic categories to solve the problem of difficulty in distinguishing wall edges from step edges in point cloud features during pure geometric analysis. Semantic-geometric joint correction reduces the terrain misclassification rate from approximately 18% to below 5%. The parameterized design of the six physical parameters adapts to different robot platforms; the output is compatible with the ROS costmap_2d interface; and the zero-derivative boundary condition eliminates cost jumps in critical regions. Attached Figure Description

[0060] Figure 1 This is a flowchart of the present invention;

[0061] Figure 2 This is a schematic diagram of the 2.5D elevation grid construction of the present invention;

[0062] Figure 3 This is a graph of the cosine soft threshold mapping function (step height sub-weights). ), marked , and zero derivative boundary conditions;

[0063] Figure 4 This is a curve of the cosine soft threshold mapping function (slope sub-weights). );

[0064] Figure 5 This is a graph of the cosine soft thresholding function (roughness sub-weights). );

[0065] Figure 6 It is a cost map based on a traditional binarization scheme;

[0066] Figure 7 This is the cost map of the gradient scheme of the present invention. Detailed Implementation

[0067] Includes the following steps:

[0068] Step 1: Elevation Grid Construction: Receive 3D point cloud data output from the 3D sensing system. This 3D point cloud data must be represented in a gravity-aligned world coordinate system. The axis points towards the zenith (the direction of anti-gravity), and the horizontal plane is defined as... Coordinate isosurfaces; the three-dimensional point cloud is projected onto a two-dimensional uniform grid on a horizontal plane to construct a 2.5D elevation grid; the grid resolution is set to... Unit: meters. The origin coordinates of the elevation grid are defined as follows: Total number of grid columns Total number of grid rows ,in ; The grid row index is denoted as ), column index is denoted as Each grid cell is denoted as For each three-dimensional point The grid cell to which it belongs is determined by the following formula: When the coordinates of a point fall exactly on the grid boundary line, according to The mathematical definition of the function belongs to the grid cell with the smaller index; for each grid cell... Statistical analysis of all point clouds falling within this grid area Coordinate values, calculate maximum elevation Minimum elevation and average elevation ,in To fall into the grid cell The number of point clouds; for The grid cells are marked as unobserved areas, i.e., non-effective grid cells;

[0069] Step 2, Step Height Feature Extraction: For each effective grid cell... , For each grid cell, calculate the maximum height abrupt change between it and its 8-neighbor grid cells. The 8-neighborhood is defined as a grid. The top ,Down ,Left ,right and top left Top right Bottom left Bottom right There are 8 adjacent grids in total. The row index of the neighboring grid is denoted as... Column index is denoted as For grid cells located at the elevation grid boundary, or ,or or Its 8-neighbor set Only include indexes in If there are fewer than 8 neighboring grids actually existing within the range, they are directly ignored; in all formulas of this method, , This indicates the row and column index of the current grid. , The row and column indices represent the neighborhood grid, and the meanings of the four indices remain consistent throughout the text; the formula for calculating the step height is:

[0070]

[0071] in For grid The 8-neighbor set of the boundary grid has fewer than 8 elements. For the current grid Average elevation value, Neighborhood grid The average elevation value;

[0072] Step 3: Local slope feature extraction: For each effective grid cell Take it as the center The average elevation data of the valid grid cells within the window; for grid cells located at the elevation grid boundary, The window only contains the index in The actual and valid grid cells within the range; when the number of valid grid cells within the window is less than 3, the slope of that grid cell. Set to 0 degrees; when the number of valid grid cells within the window... When the least squares method is used to fit the plane, the specific method is as follows: using the center coordinates of each effective grid cell within the window ( ) is the independent variable and the corresponding average elevation. Given the dependent variable, find the solution that makes the sum of squared residuals equal to... Plane parameters that achieve minimum value If ), then the normal vector of the fitting plane is , here Represents the normal vector of the fitted plane, subscript Represents "fitted", and is related to the neighborhood index in step two. The meanings are different (unnormalized), the formula for calculating the local slope angle is:

[0073]

[0074] in For the normal vector in The absolute value of the directional component. Let be the Euclidean norm of the normal vector;

[0075] Step 4: Roughness Feature Extraction: For each effective mesh cell... Calculate the root mean square deviation of all point clouds falling into the grid from the fitted plane in step three. When the mesh fails to fit the plane in step three because it has fewer than three effective neighbors, Set to 0; when the fitted plane exists, The calculation formula is:

[0076]

[0077] in ) is a point To fit the plane in step three The vertical distance is calculated using the following formula:

[0078]

[0079] in The parameters are the plane parameters obtained by least squares fitting in step three.

[0080] Step 5, Cosine Soft Threshold Mapping: Based on the mobile robot's physical mobility capabilities, three sets of physical capability parameters are pre-set: step height parameter pair ( ), slope parameter pair ( ) and roughness parameter pair ( );in This is the upper limit of the step height for robots to pass through without any cost. This represents the maximum step height that the robot can cross. The maximum slope is without cost. The maximum passable slope; The upper limit of roughness without cost. For the maximum passable roughness; for each effective mesh cell Calculate the three sub-weights separately:

[0081] Step height sub-weight Calculation of )

[0082]

[0083]

[0084]

[0085] Slope sub-weights ) and roughness sub-weights ) Adopted with The exact same cosine mapping function structure, respectively with ( )and( () as parameter pairs;

[0086] The cosine soft threshold mapping function at x = x free The function value is 0 and the first derivative dw / dx = 0 at x = x max The function value is 1 and the first derivative dw / dx = 0; the physical meaning of the zero derivative boundary condition is: the weight curve smoothly enters and exits at the two endpoints of "completely passable" and "completely impassable", with a slope of zero, to avoid cost abrupt changes at the threshold boundary;

[0087] Step Six, Probability or Fusion: Combine the three sub-weights calculated in Step Five. , , Integrate into a comprehensive accessibility weight The fusion formula is:

[0088]

[0089] in The first one calculated in step five Sub-weights of topographic features, Take the step height in sequence ,slope and roughness This fusion strategy ensures that: when any one of the sub-weights is close to 1, the overall weight increases significantly; and the overall weight is zero if and only if all three sub-weights are zero simultaneously.

[0090] Step 7, Visual Semantic Assisted Correction: Run a semantic segmentation network on an image stream synchronized with the 3D point cloud in time, classifying ground regions in the image into four semantic categories: "steps," "slopes," "flat ground," and "obstacles." Backproject the pixel-level semantic labels of the image onto an elevation grid, and apply the comprehensive weights calculated in Step 6. Make corrections, and the correction rules are as follows:

[0091]

[0092]

[0093]

[0094]

[0095] in The upper limit parameter for step cost is 0.7, with a value range of 0.5 to 0.8. The fourth rule applies to pixels with a semantic segmentation confidence of less than 0.6, which are marked as "uncertain", as well as mesh cells to which no pixels are mapped due to view occlusion or missing point clouds. In this case, the results of the geometric analysis in steps two to six are completely relied upon.

[0096] The specific calculation process for backprojecting the semantic tags onto the elevation grid is as follows: Let the image pixel coordinates be... The camera intrinsic parameter matrix is The camera's extrinsic parameters in the world coordinate system are rotation matrices. Translation vector Back projection consists of three steps: The first step is to convert pixel coordinates into normalized camera coordinates. The second step is to determine the position of the camera's optical center. As the starting point of the ray, with Using the ray direction, a spatial index is created for the 3D point cloud using a KD-Tree, with steps along the ray direction. Uniform sampling is performed, with a maximum search depth of For each sampling point, query the nearest neighbor point cloud in the KD-Tree, and select the point whose distance from the ray is less than 10 ... The nearest point cloud points are used as the back projection result If in If no point cloud point meeting the conditions is found within the range, then that pixel will not participate in semantic correction; the third step is to... Mapping to elevation grid index: When the same grid cell When semantic labels of multiple pixels are received, the final semantic category is determined by majority voting. When two or more categories have the same number of votes, the category with higher cost is selected according to the principle of safety first, with the priority from high to low as follows: obstacle > step > ramp > flat ground.

[0097] Step 8, Cost Map Generation: The adjusted comprehensive weights from Step 7 are used to generate the cost map. Mapped to the integer cost space according to the following linear mapping relationship. :

[0098]

[0099] in This indicates that passage is completely free. This indicates a fatal obstacle that prevents passage; for grid cells marked as unobserved areas in step one, a cost of 127 and a cost space are assigned. The intermediate value represents the unknown area; thus, a drivability cost map covering the entire elevation grid range is generated, which is the final output of the method of the present invention.

[0100] The unified mathematical expression for the cosine soft threshold mapping function in step five is:

[0101]

[0102]

[0103]

[0104] in For topographic features, The threshold is costless. The maximum passable threshold; in the transition zone The inner function value is strictly monotonically increasing, and at the midpoint of the interval... The maximum slope is obtained at this point. )).

[0105] The mobile robots include legged robots such as quadrupedal robot dogs and bipedal humanoid robots, as well as tracked robots and wheel-leg hybrid robots; for different robot platforms, the three sets of physical capability parameters in step five are adjusted ( ), ( ), ( This allows for adaptation to the differences in motion capabilities across different platforms without requiring modifications to the algorithm itself.

[0106] The probability or fusion strategy in step six satisfies the following three mathematical properties: 1) When hour, ;2) When , , When any one of them equals 1, 3) When all three sub-weights are between 0 and 1, Strictly greater than ).

[0107] The specific configuration of the semantic segmentation network in step seven is as follows: the network architecture adopts BiSeNet-V2, and the input resolution is [missing information]. The network was trained using a self-built dataset containing at least 5000 labeled images. These images were collected from three scenarios: urban sidewalks, indoor staircases, and construction sites, all captured using a monocular camera under natural lighting conditions. LabelMe was used for labeling, employing pixel-level semantic labels. The four label categories were defined as follows: "Steps" were ground edges with height abrupt changes exceeding 3 cm; "Ramps" were slopes with a gradient exceeding 5 degrees; "Flat Ground" was areas with a gradient less than 5 degrees and height abrupt changes less than 3 cm; and "Obstacles" included walls, railings, vehicles, and fixed man-made structures. The training framework was PyTorch, and the optimizer was SGD (learning rate 0.01, momentum 0.9, weight decay). The network is trained for 200 epochs, with data augmentation including random cropping, random horizontal flipping, and color jittering. The network output is a pixel-wise class probability map, with pixels having a confidence level below 0.6 marked as "uncertain". Alternatively, it can be pre-trained on the Cityscapes public dataset and then fine-tuned on self-built data.

[0108] In step seven The parameter selection method is as follows: In a test scenario containing known steps, for A grid search is performed from 0.4 to 0.9 with a step size of 0.05, using weighted scores. To optimize the objective, select the option that maximizes the score. The values ​​are used as default parameters; the “staircase passage success rate” is defined as the percentage of times a planned path is successfully navigated on a known passable staircase out of the total number of tests; the “obstacle crossing error rate” is defined as the percentage of times a planned path crosses a known impassable obstacle out of the total number of tests; the selection of weights 0.7 and 0.3 reflects the design principle that safety takes precedence over passage efficiency.

[0109] The method also includes a sliding window-style point cloud map update step: maintaining a point cloud map centered on the robot's current position with a radius of... Local point cloud map ( The default value is 50 meters. When the robot's movement causes the map center to shift by more than 50 meters, the robot will move to a position that is more than 50 meters away from the center of the map. When the value reaches 20%, discard the old point cloud data that exceeds the range of the new window, and integrate the newly acquired point cloud data into the current map after pose transformation. Then, re-execute steps one to eight on the updated complete local point cloud map to generate the updated cost map (i.e., full reconstruction without incremental calculation).

[0110] The 3D perception system is one of the following five types: a LiDAR-vision-inertial SLAM system, a pure LiDAR SLAM system, a pure vision SLAM system, an RGB-D depth camera system, or a pre-built static 3D point cloud map. The method requires the 3D perception system to be able to output 3D point cloud data represented in a gravity-aligned world coordinate system, with a point cloud density within a certain grid resolution. At the scale, the average number of point clouds in each grid cell must be no less than 3.

[0111] The invention will be further illustrated below with specific experimental examples.

[0112] Experiment Example 1: Cost Map Construction of a Quadruped Robot Dog in an Urban Sidewalk Environment;

[0113] Hardware configuration: The quadruped robot dog platform is equipped with a 16-line LiDAR (scanning frequency 10Hz, horizontal field of view 360 degrees, detection range 100 meters) and a global shutter monocular camera (…). The system features a pixel count, 30fps, 90-degree horizontal field of view, a six-axis IMU (200Hz sampling frequency), and an embedded computing platform (NVIDIA Jetson AGX Xavier, approximately 30 watts power consumption). The 3D perception system employs an LVI-SAM LiDAR-vision-inertial fusion SLAM framework.

[0114] (1): Set the grid resolution Meters. Urban sidewalk scene approximately Elevation grid .

[0115] (2): Typical terrain feature value - curbstone , , Sidewalk ramp , , ; Tactile paving protrudes , , .

[0116] (3): The physical ability parameters are set as follows:

[0117]

[0118] With curbstone ( For example: Therefore Therefore .

[0119] (4): The cost of the conventional solution is 254, while the cost of this invention is... .

[0120] (5): Semantic segmentation identifies the curbstone as a "step". Unchanged. The wall is identified as an "obstacle" and forced... .

[0121] (6) Output: Flat sidewalk cost 0, curb cost 89, tactile paving cost 5, ramp cost 40, wall cost 254. Construction time is approximately 15 milliseconds.

[0122] Experiment Example 2: Cost Map Construction in Ruins Scene by Tracked Search and Rescue Robot;

[0123] For the tracked robot platform, the physical capability parameters are adjusted as follows: A tilted concrete slab in the ruins ( For example: Overall weighting ,cost In the traditional solution, the cost to this area is 254 (fatal), while the cost of this invention is 182 (high cost, viable).

[0124] Experiment Example 3: Grid Resolution Impact on the accuracy of the cost map;

[0125] In the same urban pedestrian walkway scenario, respectively set Construct cost maps using 5cm, 8cm, and 10cm. Accuracy rate of 97.2% and time taken of 35ms; Accuracy of 96.1% and time taken of 15ms (recommended for outdoor urban scenarios); Accuracy rate of 93.5% and time taken of 8ms (recommended for large-scale field use); The accuracy rate was 89.7%.

[0126] Experiment Example 4: Verification of the effect of semantic-geometric joint correction;

[0127] In 100 known labeled terrain areas (30 steps, 20 ramps, 30 flat areas, and 20 obstacles), the accuracy of pure geometric analysis was 82.3% (8 wall edges were misclassified as steps and 4 low steps were misclassified as flat areas). After introducing semantic correction, the accuracy improved to 96.1% (7 of the 8 misclassified walls were corrected by the "obstacle" label).

[0128] The comparison between the present invention and the conventional solution is shown in the table below:

[0129]

[0130] The above description is merely a preferred embodiment of the present invention. The cosine soft thresholding function can be replaced by the sigmoid function, piecewise linear function, or Gaussian cumulative distribution function (CDF); probability or fusion can be replaced by weighted product, weighted summation, or fuzzy logic inference; BiSeNet-V2 can be replaced by DDRNet, PIDNet, or SegFormer. All modifications, equivalent substitutions, and improvements within the spirit and principles of this invention are included within the scope of protection.

Claims

1. A method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion, characterized in that, Includes the following steps: Step 1: Elevation grid construction; Step 2: Extraction of step height features; Step 3: Local slope feature extraction: For each effective grid cell Take it as the center The average elevation data of the valid grid cells within the window; for grid cells located at the elevation grid boundary, The window only contains the index in The actual and valid grid cells within the specified range; When the number of valid grid cells within the window is less than 3, the slope of that grid cell... Set to 0 degrees; when the number of valid grid cells within the window... When the least squares method is used to fit the plane, the specific method is as follows: using the center coordinates of each effective grid cell within the window ( ) is the independent variable and the corresponding average elevation. Given the dependent variable, find the solution that makes the sum of squared residuals equal to... Plane parameters that achieve minimum value If ), then the normal vector of the fitting plane is , here This represents the normal vector of the fitted plane, with the subscript f indicating "fitted". The formula for calculating the local slope angle is: ； in For the normal vector in The absolute value of the directional component. Let be the Euclidean norm of the normal vector; Step 4: Roughness Feature Extraction: For each effective mesh cell... Calculate the root mean square deviation of all point clouds falling into the grid from the fitted plane in step three. When the mesh fails to fit the plane in step three because it has fewer than three effective neighbors, Set to 0; when the fitted plane exists, The calculation formula is: ； in ) is a point To fit the plane in step three The vertical distance is calculated using the following formula: ； in The parameters are the plane parameters obtained by least squares fitting in step three. Step 5, Cosine Soft Threshold Mapping: Based on the mobile robot's physical mobility capabilities, three sets of physical capability parameters are pre-set: step height parameter pair ( ), slope parameter pair ( ) and roughness parameter pair ( );in This is the upper limit of the step height for robots to pass through without any cost. This represents the maximum step height that the robot can cross. The maximum slope is without cost. The maximum passable slope; The upper limit of roughness without cost. For the maximum passable roughness; for each effective mesh cell Calculate the three sub-weights separately: Step height sub-weight Calculation of ) ； ； ； Slope sub-weights ) and roughness sub-weights ) Adopted with The exact same cosine mapping function structure, respectively with ( )and( () as parameter pairs; The cosine soft threshold mapping function at x = x free The function value is 0 and the first derivative dw / dx = 0 at x = x max The function value is 1 and the first derivative dw / dx = 0; the physical meaning of the zero derivative boundary condition is: the weight curve smoothly enters and exits at the two endpoints of "completely passable" and "completely impassable", with a slope of zero, to avoid cost abrupt changes at the threshold boundary; Step Six, Probability or Fusion: Combine the three sub-weights calculated in Step Five. , , Integrate into a comprehensive accessibility weight The fusion formula is: ； in The first one calculated in step five Sub-weights of topographic features, Take the step height in sequence ,slope and roughness This fusion strategy ensures that: when any one of the sub-weights is close to 1, the overall weight increases significantly; and the overall weight is zero if and only if all three sub-weights are zero simultaneously. Step 7, Visual Semantic Assisted Correction: Run a semantic segmentation network on an image stream synchronized with the 3D point cloud in time, classifying ground regions in the image into four semantic categories: "steps," "slopes," "flat ground," and "obstacles." Backproject the pixel-level semantic labels of the image onto an elevation grid, and apply the comprehensive weights calculated in Step 6. Make corrections, and the correction rules are as follows: ； ； ； ； in The upper limit parameter for step cost is 0.7, with a value range of 0.5 to 0.

8. The fourth rule applies to pixels with a semantic segmentation confidence of less than 0.6, which are marked as "uncertain", as well as mesh cells to which no pixels are mapped due to view occlusion or missing point clouds. In this case, the results of the geometric analysis in steps two to six are completely relied upon. Step 8, Cost Map Generation: The adjusted comprehensive weights from Step 7 are used to generate the cost map. Mapped to the integer cost space according to the following linear mapping relationship. : ； in This indicates that passage is completely free. This indicates a fatal obstacle that prevents passage; for grid cells marked as unobserved areas in step one, a cost of 127 and a cost space are assigned. The intermediate value represents the unknown area; thus, a traversability cost map covering the entire elevation grid is generated.

2. The method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion according to claim 1, characterized in that, Step one specifically includes: Receive 3D point cloud data output from a 3D perception system, wherein the 3D point cloud data must be represented in a gravity-aligned world coordinate system, wherein The axis points towards the zenith, and the horizontal plane is defined as... Coordinate isosurfaces; the three-dimensional point cloud is projected onto a two-dimensional uniform grid on a horizontal plane to construct a 2.5D elevation grid; the grid resolution is set to... Unit: meters. The origin coordinates of the elevation grid are defined as follows: Total number of grid columns Total number of grid rows ,in ; The grid row index is denoted as ), column index is denoted as Each grid cell is denoted as For each three-dimensional point The grid cell to which it belongs is determined by the following formula: When the coordinates of a point fall exactly on the grid boundary line, according to The mathematical definition of the function belongs to the grid cell with the smaller index; for each grid cell... Statistical analysis of all point clouds falling within this grid area Coordinate values, calculate maximum elevation Minimum elevation and average elevation ,in To fall into the grid cell The number of point clouds; for The grid cells are marked as unobserved areas, i.e., non-effective grid cells.

3. The method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion according to claim 1, characterized in that, Step two specifically includes: For each valid grid cell , For each grid cell, calculate the maximum height abrupt change between it and its 8-neighbor grid cells. The 8-neighborhood is defined as a grid. The top ,Down ,Left ,right and top left Top right Bottom left Bottom right There are 8 adjacent grids in total. The row index of the neighboring grid is denoted as... Column index is denoted as For grid cells located at the elevation grid boundary, or ,or or Its 8-neighbor set Only include indexes in If there are fewer than 8 neighboring grids actually existing within the range, they are ignored. , This indicates the row and column index of the current grid. , The row and column indices represent the neighborhood grid, and the formula for calculating the step height is: ； in For grid The 8-neighbor set of the boundary grid has fewer than 8 elements. For the current grid Average elevation value, Neighborhood grid The average elevation value.

4. The method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion according to claim 1, characterized in that, The unified mathematical expression for the cosine soft threshold mapping function in step five is: ； ； ； in For topographic features, The threshold is costless. The maximum passable threshold; in the transition zone The inner function value is strictly monotonically increasing, and at the midpoint of the interval... The maximum slope is obtained at this point. )).

5. A method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion according to claim 1 or 4, characterized in that, The mobile robots include legged robots such as quadrupedal robot dogs and bipedal humanoid robots, as well as tracked robots and wheel-legged hybrid robots; for different robot platforms, three sets of physical capability parameters are adjusted ( ), ( ), ( This allows for adaptation to the differences in motion capabilities across different platforms without requiring modifications to the algorithm itself.

6. The method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion according to claim 1, characterized in that, The probability or fusion strategy in step six satisfies the following three mathematical properties: 1) When hour, ;2) When , , When any one of them equals 1, 3) When all three sub-weights are between 0 and 1, Strictly greater than ).

7. The method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion according to claim 1, characterized in that, The specific configuration of the semantic segmentation network in step seven is as follows: the network architecture adopts BiSeNet-V2, and the input resolution is [missing information]. The inference framework is TensorRT FP16 quantized deployment; the network training uses a self-built dataset containing no less than 5000 labeled images. The images are collected from three types of scenes: urban sidewalks, indoor stairs, and construction sites, and are taken using a monocular camera under natural lighting conditions; the annotation tool is LabelMe, and the annotation standard is pixel-level semantic labels. The criteria for the four types of labels are as follows: "steps" are ground edge areas with height changes exceeding 3 cm, "slopes" are sloping areas with continuous slopes exceeding 5 degrees, "flat ground" are areas with slopes less than 5 degrees and height changes less than 3 cm, and "obstacles" are walls, railings, vehicles, and fixed man-made facilities; The training framework is PyTorch, and the optimizer is SGD (learning rate 0.01, momentum 0.9, weight decay). The network is trained for 200 epochs, with data augmentation including random cropping, random horizontal flipping, and color jittering. The network output is a pixel-wise class probability map, with pixels with a confidence level below 0.6 marked as "uncertain". Alternatively, it can be pre-trained on the Cityscapes public dataset and then fine-tuned on self-built data.

8. A method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion according to claim 1 or 7, characterized in that, In step seven The parameter selection method is as follows: In a test scenario containing known steps, for A grid search is performed from 0.4 to 0.9 with a step size of 0.05, using weighted scores. To optimize the objective, select the option that maximizes the score. The values ​​are used as default parameters; the "staircase passage success rate" is defined as the percentage of times a planned path is successfully navigated on a known passable staircase out of the total number of tests; the "obstacle crossing error rate" is defined as the percentage of times a planned path crosses a known impassable obstacle out of the total number of tests; the selection of weights 0.7 and 0.3 reflects the design principle that safety takes precedence over passage efficiency.

9. A method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion according to claim 1 or 8, characterized in that, The specific calculation process for back-projecting the semantic tags to the elevation grid in step seven is as follows: Let the image pixel coordinates be... The camera intrinsic parameter matrix is The camera's extrinsic parameters in the world coordinate system are rotation matrices. Translation vector Back projection consists of three steps: The first step is to convert pixel coordinates into normalized camera coordinates. The second step is to determine the position of the camera's optical center. As the starting point of the ray, with Using the ray direction, a spatial index is created for the 3D point cloud using a KD-Tree, with steps along the ray direction. Uniform sampling is performed, with a maximum search depth of For each sampling point, query the nearest neighbor point cloud in the KD-Tree, and select the point whose distance from the ray is less than 10 ... The nearest point cloud points are used as the back projection result If in If no point cloud point meeting the conditions is found within the range, then that pixel will not participate in semantic correction; the third step is to... Mapping to elevation grid index: When the same grid cell When semantic labels of multiple pixels are received, the final semantic category is determined by majority voting. When two or more categories have the same number of votes, the category with higher cost is selected according to the principle of safety priority, with priority from high to low as follows: obstacle > step > ramp > flat ground.

10. The method for constructing a terrain accessibility cost map based on cosine soft thresholding and probability fusion according to claim 1, characterized in that, The method also includes a sliding window-style point cloud map update step: maintaining a point cloud map centered on the robot's current position with a radius of... Local point cloud map ( The default value is 50 meters. When the robot's movement causes the map center to shift by more than 50 meters, the robot will move to a position that is more than 50 meters away from the center of the map. When the value reaches 20%, discard the old point cloud data that exceeds the range of the new window, and integrate the newly acquired point cloud data into the current map after pose transformation. Then, re-execute steps one to eight on the updated complete local point cloud map to generate the updated cost map.

Images