An Autonomous Navigation Method for Legged Robots Based on Terrain Accessibility Weights

Abstract

This invention relates to an autonomous navigation method for legged robots based on terrain drivability weights, belonging to the field of robot autonomous navigation technology. It configures a vision-inertial subsystem and a LiDAR-inertial subsystem, outputs a 3D point cloud map through factor map fusion, obtains drivability weights through terrain analysis, and outputs a drivability cost map through path planning. The advantage is that, based on the LVI-SAM LiDAR-vision-inertial fusion SLAM framework, it innovatively introduces terrain undulation drivability weights (TTW), reassessing undulating terrain such as steps and gentle slopes, which are traditionally deemed "impassable," as "passable" terrain for legged robots. This method is particularly suitable for autonomous navigation tasks of legged robots such as robot dogs in complex terrains such as urban sidewalks, stairs, and ruins.

Description

Technical Field

[0001] This invention relates to the field of autonomous robot navigation technology, and particularly to a legged robot navigation method based on the fusion of LiDAR and vision, especially a legged robot autonomous navigation algorithm method (LVI-SAM-Trav) based on Terrain Traversability Weight (TTW). This method is mainly used for autonomous navigation tasks of legged robots such as robot dogs in complex environments such as urban sidewalks, stairs, ruins, construction sites, and unstructured terrain in the wild, providing legged robots with accurate environmental perception, terrain traversability assessment, and global path planning capabilities. Background Technology

[0002] With the rapid development of robotics technology, legged robots (such as quadrupedal robot dogs and bipedal humanoid robots) have been increasingly widely used in industrial inspection, disaster search and rescue, logistics distribution, and security patrols due to their excellent terrain adaptability. In recent years, commercial quadrupedal robot dog products, represented by Boston Dynamics Spot, Unitree Go1 / Go2, and Xiaomi CyberDog, have emerged, propelling legged robots from laboratory research to practical engineering applications. Compared to wheeled and tracked robots, legged robots possess unique mobility advantages: they can traverse steps typically 15 to 25 centimeters high by lifting their legs, maintain stable balance while walking on slopes typically 25 to 35 degrees, and adapt to irregular terrains such as gravel roads, grass, and mud by dynamically adjusting their gait and foot trajectory. These mobility capabilities give legged robots a significantly greater range of movement and task execution capabilities in complex, unstructured environments than wheeled robots. However, the full realization of the mobility of legged robots depends on the navigation system's ability to correctly identify and assess the accessibility of various terrains in the environment, providing accurate terrain difficulty information for path planning, which is precisely the weak link in current technology.

[0003] Simultaneous Localization and Mapping (SLAM) technology is the core foundation of autonomous robot navigation. In recent years, multi-sensor fusion SLAM technology has made significant progress. LVI-SAM (Lidar-Visual-Inertial SLAM via Smoothing and Mapping), proposed by Tixiao Shan et al. in 2021, is one of the most widely used LiDAR-vision fusion SLAM frameworks in academia and industry. This framework tightly couples LiDAR-Inertial Odometry (LIS) and Visual-Inertial Odometry (VIS) through a factor graph, utilizing the iSAM2 incremental smoothing optimization algorithm to achieve globally consistent state estimation. LVI-SAM possesses advantages such as modular architecture design, high-precision pose estimation, and strong robustness, enabling reliable operation in scenarios with degraded features (such as dark corridors and open spaces) through the complementary use of two subsystems.

[0004] Building upon LVI-SAM, scholars both domestically and internationally have proposed various improvement schemes for different application scenarios. Some researchers have improved the accuracy and robustness of loop closure detection by introducing semantic information, while others have enhanced the accuracy of lidar odometry by improving point cloud feature extraction and matching strategies. Still others have improved computational efficiency by optimizing the structure of factor graphs and solution algorithms. However, these improvements mainly focus on the localization and mapping accuracy of SLAM. Existing research has paid less attention to the crucial step of effectively converting the 3D map output by SLAM into navigation decision information for legged robots, especially lacking targeted modeling and utilization of the unique locomotion capabilities of legged robots.

[0005] However, existing navigation systems commonly employ a simple height threshold mechanism when converting the 3D point cloud map output by SLAM into a cost map for path planning. This mechanism typically sets a fixed height threshold (usually 5 to 10 centimeters), directly marking terrain areas exceeding this threshold as "lethal obstacles" and assigning them a maximum cost of 254 in the cost map. This binary, either-or approach is reasonable and necessary for wheeled robots, as they indeed cannot traverse obstacles exceeding a few centimeters in height. However, for legged robotic dogs, this approach is overly conservative, resulting in many terrain features that the robot could safely traverse, such as steps, curbs, and gentle slopes, being incorrectly marked as impassable areas, severely limiting the effective range of motion and task execution efficiency of legged robots.

[0006] Furthermore, existing technologies have the following prominent problems in terrain classification and judgment: First, when relying solely on point cloud geometric analysis for terrain judgment, structures such as wall edges, guardrail bottoms, and parked vehicle chassis are easily misclassified as passable steps or slopes. The lack of semantic-level auxiliary judgment capabilities leads to a high misjudgment rate and poses safety hazards. Second, existing cost maps typically only distinguish between "passable" and "impassable" states, lacking a gradient representation of terrain passability difficulty and failing to provide refined terrain difficulty information for path planning. Third, existing path planning algorithms do not explicitly consider terrain passability difficulty in cost calculation, making it impossible to reasonably balance between "choosing a longer but flatter path" and "choosing a shorter path that requires traversing steps."

[0007] In addition to the problems mentioned above, existing terrain accessibility assessment methods also face the following challenges: Fourth, existing methods generally lack modeling of the specific physical motion parameters of legged robots. Different models of robot dogs exhibit significant differences in step-crossing height, slope adaptability, and gait stability, while existing navigation systems typically use uniform fixed thresholds, failing to adaptively adjust based on the specific robot platform's motion capabilities. Fifth, the binary nature of traditional cost maps leads to a lack of flexibility in path planning results. When all possible paths are blocked by impassable areas, traditional planners can only report "no feasible path," even though the legged robot may be fully capable of traversing these misjudged areas. Sixth, in dynamic environments, traditional methods respond slowly to terrain changes and cannot update terrain accessibility assessment results in real time, affecting the real-time performance and robustness of the navigation system.

[0008] In summary, existing technologies lack a comprehensive navigation method that can quantitatively match the physical mobility of legged robots with environmental terrain features and explicitly consider terrain traversal difficulty in path planning. Specifically, existing legged robot navigation methods have three key shortcomings: (1) Insufficient assessment of the legged robot's mobility capabilities, failing to incorporate physical parameters such as the robot's maximum traversable step height and maximum traversable slope into terrain traversability judgment; (2) Overly conservative terrain traversability judgment, using binary height thresholds instead of continuously gradient-based weight evaluation, leading to many traversable areas being misjudged as fatal obstacles; (3) Path planning lacks terrain perception capabilities, with cost functions only considering geometric distances without modeling terrain traversal difficulty, failing to quantitatively balance path length and terrain difficulty. Therefore, a new navigation algorithm is urgently needed to fully leverage the mobility potential of legged robots and maximize their traversability and navigation efficiency in complex terrain while ensuring safety. Summary of the Invention

[0009] This invention provides an autonomous navigation method for legged robots based on terrain accessibility weights. It addresses the three key shortcomings of existing technologies by solving the problems of insufficient assessment of the legged robot's motion capabilities, overly conservative judgment of terrain accessibility, and lack of terrain perception capability in path planning.

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

[0011] (1) Configure the Vision-Inertial Subsystem (VIS): It is equipped with a monocular camera and an inertial measurement unit (IMU) to extract image feature points. It provides short-term motion priors through IMU pre-integration and performs nonlinear optimization by minimizing the joint cost function of visual reprojection error and IMU pre-integration residual within a sliding window to obtain local high-frequency pose estimation. At each key frame, the Vision-Inertial Subsystem encapsulates the relative pose constraints between adjacent key frames obtained by the above nonlinear optimization into a visual inertial odometry factor containing rotation matrix, velocity vector and displacement vector, and passes it to the factor graph fusion module.

[0012] (2) Configure the lidar-inertial subsystem (LIS): It is equipped with lidar sensors and inertial measurement units to perform voxel downsampling and feature classification on the original point cloud. The point cloud is divided into two categories: edge features and planar features. The weighted sum of the distance residuals from edge feature points to corresponding edge lines and the distance residuals from planar feature points to corresponding planes is minimized by scan-to-map matching. The 6-DOF pose transformation matrix that minimizes the sum of the residuals is solved, and the lidar odometry factor is output.

[0013] (3) Factor graph fusion: Connect the vision-inertial subsystem and the lidar-inertial subsystem, and model the odometry factor and odometry factor output by the two subsystems into factor nodes in the factor graph. Perform global joint optimization through iSAM2 incremental smoothing and graph building algorithm to output a globally consistent robot state estimate and a three-dimensional point cloud map containing six degrees of freedom pose, three-dimensional velocity and IMU bias of three-dimensional position and three-dimensional attitude.

[0014] (4) Terrain Analysis (TAM): Receives the 3D point cloud map output from the factor map fusion step, constructs a 2.5D elevation grid, where the elevation grid projects the 3D point cloud onto a 2D uniform grid on a horizontal plane, with a grid resolution of [resolution missing]. The grid row index is denoted as Column index is denoted as Each grid cell is denoted as For each grid cell, three terrain features are extracted: step height, local slope, and roughness. The accessibility sub-weights corresponding to each feature are calculated using a cosine soft threshold mapping function. A probability or fusion strategy is used to calculate the comprehensive accessibility weight. Then, the semantic-geometric joint correction is performed by combining the visual semantic segmentation results, and finally, the accessibility cost map is output.

[0015] (5) Path planning: Based on the drivability cost map output from the terrain analysis step, the TTW-A* path planning algorithm is used to plan the path in the standard... The algorithm's cost function incorporates terrain accessibility weights as a penalty term, and its cost accumulation formula is as follows: ),in This represents the Euclidean distance between the current node and its parent node. This represents the overall accessibility weight of the grid corresponding to the current node. The terrain penalty coefficient is used to generate the path with the minimum total weighted cost that takes into account terrain décor, where the total weighted cost is defined as the edge cost between all adjacent nodes on the path. The cumulative summation of the values ​​ensures that the legged robot can select the route with the minimum total weighted cost in complex terrain environments containing steps, gentle slopes, and rough ground.

[0016] The specific processing flow of the vision-inertial subsystem described in this invention involves extracting sparse image feature points using the BRIEF descriptor, performing inter-frame feature tracking using the KLT optical flow method, and using the IMU pre-integration factor as a short-term motion constraint. Within a sliding window optimization framework, it jointly optimizes camera pose, landmark positions, and IMU bias parameters. The BRIEF descriptor generates a fixed-length binary feature vector by comparing the brightness of randomly selected pixel pairs within a local area of ​​the image, which is used to quickly match corresponding feature points between adjacent frames. The KLT optical flow method, based on the Lucas-Kanade optical flow model, assumes that the neighborhood grayscale of the same feature point remains consistent between adjacent frames. It iteratively solves for the sub-pixel displacement of the feature point between adjacent frames by minimizing the template matching residual. The IMU pre-integration integrates all IMU acceleration and angular velocity measurement data between two keyframes into a relative rotation increment without relying on absolute pose. Relative velocity increment and relative displacement increment The aforementioned increments are encapsulated as constraint factors in a factor graph; the sliding window optimization maintains a fixed number of recent keyframes, with a window size of [missing value]. Within the window, the joint cost function of visual reprojection error and IMU pre-integration residual is minimized using the Gauss-Newton or Levenberg-Marquardt algorithm. When a new keyframe is added, the oldest keyframe in the window is marginalized to control the computational scale.

[0017] The lidar-inertial subsystem described in this invention performs curvature analysis on the original point cloud, classifying high-curvature points as edge features and low-curvature points as planar features. Through scan-to-map matching with the global feature map, it minimizes the weighted distance residuals from points to edge lines and from points to planes, and solves for the 6-DOF pose transformation matrix in the current frame that minimizes the sum of these weighted distance residuals. The specific method of curvature analysis is as follows: for each point in a single lidar frame scan, the rate of change of distance between that point and its five adjacent points along the scan line direction is calculated as the curvature estimate for that point. Points with curvature values ​​exceeding a set threshold are identified... Points with curvature values ​​below a set threshold are classified as edge feature points, and points with curvature values ​​below a set threshold are classified as planar feature points. The specific method of scan-to-map matching is as follows: the edge feature points and planar feature points extracted in the current frame are respectively associated with the existing edge line segments and planes in the global feature map through nearest neighbor relationships. The distance residual of an edge feature point is defined as the point-to-line distance from the point to the corresponding edge line segment, and the distance residual of a planar feature point is defined as the point-to-plane distance from the point to the corresponding plane. The 6-DOF pose transformation matrix that minimizes the weighted sum of the above two types of distance residuals is solved iteratively using the Levenberg-Marquardt nonlinear optimization algorithm. ,in Let be the pose transformation matrix to be optimized. For edge feature points, For planar feature points, and These are the distance functions from the point to the edge line and from the point to the plane, respectively.

[0018] The terrain analysis step (TAM) described in this invention performs accessibility analysis on 3D point clouds, employing a terrain accessibility cost map construction method based on cosine soft thresholding and probability fusion, as detailed below:

[0019] Step 1: Elevation Grid Construction: Receive the 3D point cloud map data output from the factor map fusion step. The 3D point cloud data must be represented in a gravity-aligned world coordinate system. The axis points towards the zenith and the anti-gravity direction; the horizontal plane is defined as... Coordinate isosurfaces; the 3D point cloud is projected onto a 2D uniform grid on a horizontal plane to construct a 2.5D elevation grid; the grid resolution is r, in meters, and the origin coordinates of the elevation grid are defined as... Input point cloud Input point cloud 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;

[0020] 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:

[0021]

[0022] 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;

[0023] Step 3: Local slope feature extraction: For each effective grid cell Take it as the center Average elevation data of the effective grid cells within the window. For grid cells located at elevation grid boundaries, 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 Representing "fitted", the formula for calculating the local slope angle is:

[0024]

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

[0026] 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:

[0027]

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

[0029]

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

[0031] Step 5, Cosine Soft Threshold Mapping: Based on the legged 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 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:

[0032] Step height sub-weight Calculation of )

[0033]

[0034]

[0035]

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

[0037] The cosine soft threshold mapping function at x = The function value is 0 and the first derivative dw / dx = 0 at x = 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;

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

[0039]

[0040] 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.

[0041] Step 7, Visual Semantic Assisted Correction: Run a semantic segmentation network on the image stream of the visual-inertial subsystem to classify the 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 the elevation grid and apply the comprehensive weights calculated in Step 6. Make corrections, and the correction rules are as follows:

[0042]

[0043]

[0044]

[0045]

[0046] 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.

[0047] 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 [0, 0, 1]], where the extrinsic parameters of the camera 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 midpoint to the ray is less than 1 / 2. 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.

[0048] 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. :

[0049]

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

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

[0052]

[0053]

[0054]

[0055] 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. ));

[0056] The legged robots include quadrupedal robotic 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.

[0057] In step six of this invention, the probability or fusion strategy satisfies the following three mathematical properties: (i) when hour, (ii) When , , When any one of them equals 1, (iii) When all three sub-weights are between 0 and 1, Strictly greater than ).

[0058] 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;

[0059] 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, and were taken using a monocular camera under natural lighting conditions. LabelMe was used for labeling, with 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, with SGD as the optimizer, a learning rate of 0.01, momentum of 0.9, and 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.

[0060] This invention 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... 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).

[0061] The TTW-A* path planning algorithm in path planning described in this invention (5) modifies the cost function based on the standard A* algorithm as follows:

[0062]

[0063] in For nodes Total estimated cost, From the starting point to the node The cumulative actual cost, For the node Heuristic cost estimation to the destination; The accumulation method has been modified as follows:

[0064]

[0065] in The cumulative cost of the parent node. This represents the Euclidean distance between the current node and its parent node. This represents the overall accessibility weight of the grid corresponding to the current node. The terrain penalty coefficient; when When the value is large, the planner tends to choose flat routes to avoid terrain obstacles; when When a smaller value is selected, the planner is more inclined to bypass the steps to shorten the journey, thereby achieving an adjustable total weighted cost minimization trade-off between "taking a detour on flat ground" and "going straight through the steps".

[0066] The terrain penalty coefficient described in this invention The value ranges from 1.0 to 10.0 and is dynamically adjusted according to task requirements and the robot's motion capabilities: when the task prioritizes motion safety, Take the larger value; when path efficiency is prioritized in the task. Take the smaller value; the heuristic cost estimation Use Euclidean distance or diagonal distance to ensure that the heuristic function satisfies the consistency condition.

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

[0068] First, by innovatively introducing the continuous drivability weight function TTW, and replacing the traditional binary obstacle judgment with cosine soft threshold mapping and probability OR fusion, the effective range of motion of the legged robot is expanded by more than 40%, which greatly improves the robot dog's freedom of movement in urban environments.

[0069] Second, a semantic-geometric joint terrain classification method is adopted, which deeply integrates point cloud geometric analysis with visual semantic segmentation information. This effectively solves the problem that single geometric analysis can easily misjudge structures such as wall edges and the bottom of guardrails as steps, reducing the terrain misjudgment rate to below 5% and significantly improving navigation safety.

[0070] Third, by using the terrain-aware path planning TTW-A* algorithm, terrain accessibility weights are explicitly introduced into the path cost function, enabling the path planner to make an optimal trade-off between "detouring on flat roads" and "directly traversing steps," improving path planning efficiency by more than 30% and significantly increasing task completion rate.

[0071] Fourth, the system adopts a modular architecture design. The terrain analysis module and the TTW-A* path planning module can be integrated into other SLAM frameworks as independent plug-ins, which has good scalability and engineering practicality.

[0072] Fifth, the cosine soft threshold mapping function has the mathematical property that its derivative is zero at the threshold boundary, which ensures a smooth transition of the drivability weight between "fully drivable" and "inaccessible", avoiding the cost jump phenomenon in the critical region caused by the traditional hard threshold method, and making the path planning results more stable and predictable.

[0073] Sixth, the probabilistic or fusion strategy is mathematically equivalent to assuming that the inaccessibility determination of terrain features in each dimension is an independent event. It has a clear probabilistic interpretation and satisfies the "safety first" principle—any dimension approaching inaccessibility significantly increases the overall cost, effectively preventing the risks of certain dimensions from being masked by the low weights of other dimensions. Compared to simple fusion methods such as weighted summation or taking the maximum value, this fusion strategy achieves a better balance between safety and accessibility.

[0074] Seventh, the overall computation time of the system on the embedded platform is kept within a reasonable range. The frame processing time of the LVI-SAM fusion SLAM module is approximately 40 milliseconds, the processing time of the terrain analysis module (including elevation grid construction, feature extraction, weight calculation, and semantic correction) is approximately 15 milliseconds, and the TTW-A* path planning module has a frame processing time of approximately 100 milliseconds. The single planning time on the cost map with a range of meters and a grid resolution of 5 centimeters is approximately 10 milliseconds, and the overall system frame rate can reach over 15Hz, meeting the requirements of real-time navigation. The calculations of the terrain analysis module and the path planning module mainly involve grid traversal and numerical operations, which can run efficiently on ARM processors without GPU acceleration, providing excellent conditions for embedded deployment. Attached Figure Description

[0075] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this application, illustrate exemplary embodiments of the invention and are used to explain the invention, but do not constitute an undue limitation of the invention.

[0076] Figure 1 This is a system architecture diagram of the present invention, showing the data flow and interface relationships between the Vision-Inertial Subsystem (VIS), the LiDAR-Inertial Subsystem (LIS), the Factor Graph Fusion Module, the Terrain Analysis Module (TAM), and the Path Planning Module;

[0077] Figure 2 This is a schematic diagram of the 2.5D elevation grid construction of the present invention, which shows the process of projecting a three-dimensional point cloud onto a two-dimensional grid on a horizontal plane and calculating the elevation statistics of each grid cell;

[0078] Figure 3 This is a graph of the cosine soft threshold mapping function (step height sub-weights) of the present invention.

[0079] Figure 4 This is the cosine soft threshold mapping function curve (slope sub-weight) of the present invention.

[0080] Figure 5 This is a graph of the cosine soft threshold mapping function (roughness sub-weights) of the present invention.

[0081] Figure 6 This is a schematic diagram of the cost map of the conventional solution of the present invention (the step area is a completely black fatal obstacle).

[0082] Figure 7 This is a schematic diagram of the cost map of the present invention (the step area is represented by gradient grayscale).

[0083] Figure 8 This is a diagram illustrating the TTW-A* path planning effect of the present invention. (Time-based travel strategy path).

[0084] Figure 9 This is a diagram illustrating the TTW-A* path planning effect of the present invention. Equilibrium strategy path at the time).

[0085] Figure 10 This is a diagram illustrating the TTW-A* path planning effect of the present invention. (A conservative strategy path at that time). Detailed Implementation

[0086] The technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0087] Includes the following steps:

[0088] (1) Configure the Vision-Inertial Subsystem (VIS): It is equipped with a monocular camera and an inertial measurement unit (IMU) to extract image feature points. It provides short-term motion priors through IMU pre-integration and performs nonlinear optimization by minimizing the joint cost function of visual reprojection error and IMU pre-integration residual within a sliding window to obtain local high-frequency pose estimation. At each key frame, the Vision-Inertial Subsystem encapsulates the relative pose constraints between adjacent key frames obtained by the above nonlinear optimization into a visual inertial odometry factor containing rotation matrix, velocity vector and displacement vector, and passes it to the factor graph fusion module.

[0089] (2) Configure the lidar-inertial subsystem (LIS): It is equipped with lidar sensors and inertial measurement units to perform voxel downsampling and feature classification on the original point cloud. The point cloud is divided into two categories: edge features and planar features. The weighted sum of the distance residuals from edge feature points to corresponding edge lines and the distance residuals from planar feature points to corresponding planes is minimized by scan-to-map matching. The 6-DOF pose transformation matrix that minimizes the sum of the residuals is solved, and the lidar odometry factor is output.

[0090] (3) Factor graph fusion: Connect the vision-inertial subsystem and the lidar-inertial subsystem, and model the odometry factors and odometry factors output by the two subsystems into factor nodes in the factor graph. Perform global joint optimization through iSAM2 incremental smoothing and graph building algorithm to output a globally consistent robot state estimate and a three-dimensional point cloud map containing six degrees of freedom pose (three-dimensional position and three-dimensional attitude), three-dimensional velocity and IMU bias.

[0091] (4) Terrain Analysis (TAM): Receives the 3D point cloud map output from the factor map fusion step, constructs a 2.5D elevation grid, where the elevation grid projects the 3D point cloud onto a 2D uniform grid on a horizontal plane, with a grid resolution of [resolution missing]. The grid row index is denoted as Column index is denoted as Each grid cell is denoted as For each grid cell, three terrain features are extracted: step height, local slope, and roughness. The accessibility sub-weights corresponding to each feature are calculated using a cosine soft threshold mapping function. A probability or fusion strategy is used to calculate the comprehensive accessibility weight. Then, the semantic-geometric joint correction is performed by combining the visual semantic segmentation results, and finally, the accessibility cost map is output.

[0092] (5) Path planning: Based on the drivability cost map output from the terrain analysis step, the TTW-A* path planning algorithm is used to plan the path in the standard... The algorithm's cost function incorporates terrain accessibility weights as a penalty term, and its cost accumulation formula is as follows: ),in This represents the Euclidean distance between the current node and its parent node. This represents the overall accessibility weight of the grid corresponding to the current node. The terrain penalty coefficient is used to generate the path with the minimum total weighted cost that takes into account terrain décor, where the total weighted cost is defined as the edge cost between all adjacent nodes on the path. The cumulative summation of the values ​​ensures that the legged robot can select the route with the minimum total weighted cost in complex terrain environments containing steps, gentle slopes, and rough ground.

[0093] The specific processing flow of the vision-inertial subsystem described in this invention involves extracting sparse image feature points using the BRIEF descriptor, performing inter-frame feature tracking using the KLT optical flow method, and using the IMU pre-integration factor as a short-term motion constraint. Within a sliding window optimization framework, it jointly optimizes camera pose, landmark positions, and IMU bias parameters. The BRIEF descriptor generates a fixed-length binary feature vector by comparing the brightness of randomly selected pixel pairs within a local area of ​​the image, which is used to quickly match corresponding feature points between adjacent frames. The KLT optical flow method, based on the Lucas-Kanade optical flow model, assumes that the neighborhood grayscale of the same feature point remains consistent between adjacent frames. It iteratively solves for the sub-pixel displacement of the feature point between adjacent frames by minimizing the template matching residual. The IMU pre-integration integrates all IMU acceleration and angular velocity measurement data between two keyframes into a relative rotation increment without relying on absolute pose. Relative velocity increment and relative displacement increment The aforementioned increments are encapsulated as constraint factors in a factor graph; the sliding window optimization maintains a fixed number of recent keyframes, with a window size of [missing value]. Within the window, the joint cost function of visual reprojection error and IMU pre-integration residual is minimized using the Gauss-Newton or Levenberg-Marquardt algorithm. When a new keyframe is added, the oldest keyframe in the window is marginalized to control the computational scale.

[0094] The state propagation equation of the vision-inertial subsystem is based on the IMU pre-integration model, for two keyframes. and Pre-integrate the inertial measurement data between the two sides to obtain the relative rotation increment. Relative velocity increment and relative displacement increment The state propagation equation is:

[0095]

[0096]

[0097]

[0098] in , , These represent the predicted rotation matrix, velocity vector, and displacement vector, respectively. The vector of gravitational acceleration. The time interval between two frames is denoted as . The advantage of the pre-integration model is that when the keyframe pose is updated by the backend optimization, there is no need to re-integrate the original IMU data; the pre-integrated quantity can be efficiently updated simply by using a first-order correction.

[0099] The lidar-inertial subsystem described in this invention performs curvature analysis on the original point cloud, classifying high-curvature points as edge features and low-curvature points as planar features. Through scan-to-map matching with the global feature map, it minimizes the weighted distance residuals from points to edge lines and from points to planes, and solves for the 6-DOF pose transformation matrix in the current frame that minimizes the sum of these weighted distance residuals. The specific method of curvature analysis is as follows: for each point in a single lidar frame scan, the rate of change of distance between that point and its five adjacent points along the scan line direction is calculated as the curvature estimate for that point. Points with curvature values ​​exceeding a set threshold are identified... Points with curvature values ​​below a set threshold are classified as edge feature points, and points with curvature values ​​below a set threshold are classified as planar feature points. The specific method of scan-to-map matching is as follows: the edge feature points and planar feature points extracted in the current frame are respectively associated with the existing edge line segments and planes in the global feature map through nearest neighbor relationships. The distance residual of an edge feature point is defined as the point-to-line distance from the point to the corresponding edge line segment, and the distance residual of a planar feature point is defined as the point-to-plane distance from the point to the corresponding plane. The 6-DOF pose transformation matrix that minimizes the weighted sum of the above two types of distance residuals is solved iteratively using the Levenberg-Marquardt nonlinear optimization algorithm. ,in Let be the pose transformation matrix to be optimized. For edge feature points, For planar feature points, and These are the distance functions from the point to the edge line and from the point to the plane, respectively.

[0100] The joint optimization objective function for factor graph fusion in this invention is:

[0101]

[0102] in The set of all state variables to be optimized (including pose, velocity, and IMU bias of all keyframes). For the first The residual function corresponding to each factor This is the information matrix (inverse of the covariance matrix) of the factor. Factor types in the factor graph include: IMU pre-integration factors, visual odometry factors, and lidar odometry factors. The iSAM2 algorithm achieves efficient online optimization through an incremental Bayesian tree structure, performing local relinearization and back-substitution only on variables affected by new factors. This avoids the high computational cost of globally optimizing the entire factor graph each time a new observation is added, ensuring the system's real-time operation capability on embedded platforms.

[0103] Regarding the collaborative mechanism between the vision-inertial subsystem and the lidar-inertial subsystem, the two subsystems achieve tight coupling by sharing keyframe pose nodes through factor maps. When visual feature tracking fails (e.g., in low-light environments or regions lacking texture), the odometry factors of the lidar-inertial subsystem can maintain the continuity of pose estimation; when lidar features degrade (e.g., in environments with insufficient geometric features such as long corridors or open fields), the factors of the vision-inertial subsystem can provide effective pose constraints. This complementary mechanism significantly improves the robustness and reliability of the SLAM system in complex and variable environments.

[0104] The terrain analysis step (TAM) described in this invention performs accessibility analysis on 3D point clouds, employing a terrain accessibility cost map construction method based on cosine soft thresholding and probability fusion, as detailed below:

[0105] Step 1: Elevation Grid Construction: Receive the 3D point cloud map data output from the factor map fusion step. The 3D point cloud data must be represented in a gravity-aligned world coordinate system. The axis points towards the zenith and the anti-gravity direction; the horizontal plane is defined as... Coordinate isosurfaces; the 3D point cloud is projected onto a 2D uniform grid on a horizontal plane to construct a 2.5D elevation grid; the grid resolution is r, in meters, and the origin coordinates of the elevation grid are defined as... Input point cloud Input point cloud 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., ineffective grid cells).

[0106] 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:

[0107]

[0108] 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;

[0109] Step 3: Local slope feature extraction: For each effective grid cell Take it as the center Average elevation data of the effective grid cells within the window. For grid cells located at elevation grid boundaries, 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. (Different meanings) (unnormalized), the formula for calculating the local slope angle is:

[0110]

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

[0112] 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:

[0113]

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

[0115]

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

[0117] Step 5, Cosine Soft Threshold Mapping: Based on the legged 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 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:

[0118] Step height sub-weight Calculation of )

[0119]

[0120]

[0121]

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

[0123] The cosine soft threshold mapping function at x = The function value is 0 and the first derivative dw / dx = 0 at x = 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;

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

[0125]

[0126] 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.

[0127] Step 7, Visual Semantic Assisted Correction: Run a semantic segmentation network on the image stream of the visual-inertial subsystem to classify the 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 the elevation grid and apply the comprehensive weights calculated in Step 6. Make corrections, and the correction rules are as follows:

[0128]

[0129]

[0130]

[0131]

[0132] 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.

[0133] 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 [0, 0, 1]], where the extrinsic parameters of the camera 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 midpoint to the ray is less than 1 / 2. 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.

[0134] 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. :

[0135]

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

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

[0138]

[0139]

[0140]

[0141] 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. ));

[0142] The legged robots include quadrupedal robotic 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.

[0143] In step six of this invention, the probability or fusion strategy satisfies the following three mathematical properties: (i) when hour, (ii) When , , When any one of them equals 1, (iii) When all three sub-weights are between 0 and 1, Strictly greater than ).

[0144] 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;

[0145] 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, and were taken using a monocular camera under natural lighting conditions. LabelMe was used for labeling, with 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, with SGD as the optimizer, a learning rate of 0.01, momentum of 0.9, and 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.

[0146] 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.

[0147] This invention 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... 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).

[0148] The TTW-A* path planning algorithm in path planning described in this invention (5) modifies the cost function based on the standard A* algorithm as follows:

[0149]

[0150] in For nodes Total estimated cost, From the starting point to the node The cumulative actual cost, For the node Heuristic cost estimation to the destination; The accumulation method has been modified as follows:

[0151]

[0152] in The cumulative cost of the parent node. This represents the Euclidean distance between the current node and its parent node. This represents the overall accessibility weight of the grid corresponding to the current node. The terrain penalty coefficient; when When the value is large, the planner tends to choose flat routes to avoid terrain obstacles; when When a smaller value is selected, the planner is more inclined to bypass the steps to shorten the journey, thereby achieving an adjustable total weighted cost minimization trade-off between "taking a detour on flat ground" and "going straight through the steps".

[0153] The terrain penalty coefficient described in this invention The value ranges from 1.0 to 10.0 and is dynamically adjusted according to task requirements and the robot's motion capabilities: when the task prioritizes motion safety, Take the larger value; when path efficiency is prioritized in the task. Take the smaller value; the heuristic cost estimation Use Euclidean distance or diagonal distance to ensure that the heuristic function satisfies the consistency condition.

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

[0155] Experiment Example 1: Autonomous Navigation of Urban Pedestrian Streets

[0156] This experimental example uses the autonomous navigation task of a quadruped robot dog in an urban sidewalk environment to describe in detail the workflow and parameter configuration of each step of the method.

[0157] Hardware configuration: The quadruped robot dog platform used in this experiment is equipped with a 16-line mechanical rotating lidar with a scanning frequency of 10Hz, a horizontal field of view of 360 degrees, and a detection range of 100 meters; and a global shutter monocular camera with a resolution of [missing information]. Pixels, frame rate 30fps, horizontal field of view 90 degrees; one six-axis inertial measurement unit (IMU), sampling frequency 200Hz, accelerometer range [missing information]. The gyroscope range is Degrees / second; an airborne embedded computing platform equipped with an ARM processor and GPU acceleration module. All sensors have undergone rigorous time synchronization calibration and spatial extrinsic calibration to ensure accurate alignment of multi-source data in time and space. The overall system power consumption is about 35 watts, which meets the airborne power supply limitations of the robot dog platform.

[0158] 1): Elevation grid construction. Set the grid resolution. centimeters, grid row index Column index The raw 3D point cloud acquired by the lidar is first downsampled using voxel filtering (voxel size is 2 cm), and then processed by the LVI-SAM fusion system to obtain a globally consistent 3D point cloud map. The point cloud map is then projected onto a 2.5D elevation grid on a horizontal plane, and for each grid cell... Statistical analysis of all point clouds falling into this grid Coordinate values, calculate maximum elevation Minimum elevation and average elevation For grid cells with fewer than 3 points falling into them, they are marked as unobserved areas and assigned an unknown cost in the cost map.

[0159] 2): Topographic feature extraction. Three key topographic features are extracted for each effective grid cell: (1) Step height This feature is obtained by calculating the maximum average elevation difference between the current grid and its 8 neighboring grids. It can effectively capture abrupt changes in elevation in the terrain, such as curbs and steps. (2) Local slope By focusing on the current grid The elevation data within the window (9 grid cells in total) is fitted to a plane using the least squares method. The normal vector of this plane and the vertical direction are calculated. (3) Roughness It is obtained by calculating the root mean square deviation of all point clouds within a local window from the fitted plane, reflecting the irregularity and microscopic unevenness of the ground.

[0160] 3): Physical capability parameter settings and cosine soft threshold mapping. Based on the motion capability specifications of the quadruped robot dog platform and actual test results, the following physical capability parameters are set:

[0161]

[0162] Among the parameters above, "no-cost threshold" represents the upper limit of terrain features that the robot dog can freely pass through without any additional cost, and "maximum threshold" represents the physical limit of terrain features that the robot dog can pass through. If the value is exceeded, it is determined to be impassable.

[0163] Furthermore, the step height sub-weight is calculated for each grid cell. .when When less than or equal to 5 cm, This indicates that passage is completely free and incurs no additional cost. When For values ​​greater than 5 cm and less than or equal to 20 cm, the cosine soft threshold mapping function is used for calculation. This achieves a smooth and continuous transition from freely passable to impassable. When it is greater than 20 cm, This indicates that the height of the step exceeds the robot dog's physical crossing capability, and it is determined to be impassable. Slope sub-weight and roughness sub-weights Using the exact same cosine mapping function structure, calculations are performed using the corresponding physical capability parameters.

[0164] Furthermore, a probabilistic or fusion strategy is adopted. Calculate the overall accessibility weights. The following example illustrates the effect of this fusion strategy: For a step with a height of 12 centimeters ( The slope is 5 degrees. ), roughness is 1 cm ( ) grid cells, comprehensive weights The corresponding cost is approximately 89, far lower than the traditional solution's 254 (fatal), and the area can be safely traversed by the robot dog. For a step with a height of 18 centimeters... The slope is 15 degrees. The surface roughness is 3 cm. ) grid cells, comprehensive weights The corresponding cost is approximately 232. Although the cost is high, it is still not a fatal obstacle of 254, indicating that the robot dog can still attempt to pass through the area in an emergency.

[0165] 4): Probabilistic or fusion-based visual semantic-assisted correction. The BiSeNet-V2 lightweight semantic segmentation network is run on the image stream of the vision-inertial subsystem. This network can run in real-time on an embedded GPU at approximately 60fps, classifying ground regions in the image into four semantic categories: "steps," "slopes," "flat ground," and "obstacles." Using the calibrated camera intrinsic matrix and the camera extrinsic parameters (pose relative to the robot's body coordinate system) estimated by the SLAM system, pixel-level semantic labels in the image coordinate system are back-projected into the 3D point cloud coordinate system, and then mapped to the corresponding elevation grid cells. The semantic-geometric joint correction rule is as follows: For steps identified as "steps" by visual semantic segmentation and whose height is obtained from geometric analysis... No more than The grid cells are constrained to have their composite weights limited to The following measures prevent excessive penalties caused by point cloud acquisition noise and voxelization errors. For regions identified as "obstacles" (such as walls, fences, parked vehicles, etc.), their weight is maintained at 1.0 (impassable) regardless of the geometric analysis results to ensure safety. For other categories, the original geometric analysis weights remain unchanged.

[0166] 5): Cost map generation and path planning. The comprehensive weights, after semantic-geometric joint correction... Mapped to the integer cost space according to a linear mapping relationship. The specific mapping formula is as follows: Generate a accessibility cost map. Run the TTW-A* path planner on this cost map, setting terrain penalty coefficients. During the search process, the planner calculates the cumulative cost, including terrain penalties, for each candidate expansion node. ), and uses Euclidean distance as a heuristic function. According to the total cost The planner expands by selecting nodes from the open list in ascending order. In this way, the planner can automatically make the optimal trade-off between "taking a detour on a flat road" (longer distance but lower terrain cost) and "directly crossing the steps" (shorter distance but higher terrain cost), generating the global path with the lowest total cost.

[0167] In the actual test of this experiment, in an urban sidewalk environment containing curbs (approximately 12 cm high), sidewalk ramps (approximately 15 degrees in slope), and tactile paving protrusions (approximately 0.5 cm high), compared to a navigation scheme using a traditional height threshold cost map, the robot dog's effective range of motion increased by approximately 45%, the path planning success rate improved from 72% to 98%, and the average path length was shortened by approximately 20%. It successfully completed several tasks involving traversing areas previously marked as impassable by traditional methods. During specific task execution, the robot dog was able to autonomously identify curbs and select appropriate crossing points, maintaining a stable gait and speed when ascending and descending sidewalk ramps. The entire navigation process required no manual remote control intervention, achieving truly fully autonomous navigation. The system's average frame processing time was 65 milliseconds, meeting the requirements for real-time navigation.

[0168] Experiment Example 2: Autonomous Navigation in Indoor Staircase Environment

[0169] This experimental example uses a quadruped robot dog's autonomous navigation task in an indoor staircase environment to demonstrate the system's performance in scenarios with numerous high steps.

[0170] Indoor staircase environments have the following characteristics: step height is typically 14 to 18 cm, step width is typically 25 to 30 cm, the overall inclination angle of the staircase is typically 25 to 35 degrees, and there are walls or railings on both sides of the staircase. Furthermore, the lidar point cloud features of indoor staircase environments exhibit high repeatability, with adjacent steps having extremely similar geometric structures, placing high demands on the accuracy of loop closure detection and pose estimation in the SLAM system. This scenario also places higher demands on the terrain analysis module: on the one hand, it needs to accurately identify continuous step structures and assess their accessibility; on the other hand, it needs to correctly identify the walls on both sides of the staircase as impassable obstacles rather than steps. In particular, the metal bars of stair handrails typically appear as sparse discrete points in lidar scans, easily removed by voxel filtering or ignored by elevation grids; therefore, visual semantic segmentation plays an irreplaceable auxiliary role in identifying handrail structures.

[0171] In this experimental example, the physical ability parameters are set the same as in Experiment 1. In the staircase environment, the elevation grid resolution is adjusted to... Centimeters are used to obtain a more detailed topographic representation. Because the height of each step in a staircase environment is typically 15 to 17 centimeters (in centimeters). arrive Between 0.6 and 0.8), the cosine soft thresholding function will calculate a sub-weight of approximately 0.6 to 0.8 for each step grid cell, corresponding to the medium-to-high cost region.

[0172] In the visual semantic-assisted correction stage, the BiSeNet-V2 network can effectively identify staircase step structures and assign the semantic label "step" to the corresponding elevation grid. For grid cells identified as steps, the overall weights are limited to... Below. Simultaneously, the walls on both sides of the staircase are correctly identified as "obstacles," with their weights forcibly set to 1.0. This semantic-geometric joint judgment effectively avoids the problem of misclassifying wall edges as passable steps.

[0173] During the path planning phase, the terrain penalty coefficient is set to... (Reduced compared to Example 1), allowing the planner to more actively select paths through the stairs. Test results show that the robot dog can successfully plan and execute navigation tasks up and down stairs on standard indoor stairs (step height 16 cm, width 28 cm), with a success rate of over 95% on a single-story staircase (approximately 12 steps). In tests on a continuous multi-story staircase (3 floors, 36 steps in total), the system-planned path correctly guides the robot dog through the turning areas of each step and stair landing, achieving a success rate of over 90% and an average travel time of approximately 180 seconds, significantly outperforming traditional solutions that require manual remote control.

[0174] Example 3: Terrain Penalty Coefficient Comparative experiment

[0175] This effect demonstration example showcases the terrain penalty coefficient through comparative experiments. Impact on path planning results. The experimental scenario is a city sidewalk with two selectable paths: Path The route is a flat bypass, with a total distance of approximately 120 meters and a terrain weight of 0 for all sections. The route is a straight path, with a total distance of about 60 meters, but it requires crossing two levels of curb stones (about 12 centimeters high, with a weight of about 0.35) and a gentle slope (about 12 degrees, with a weight of about 0.02).

[0176] when At that time, path The total cost is ,path The total cost is The planner selects the path. (Crossing the stairs) reduces the total cost by 31.5%. When At that time, path The total cost is ,path The total cost remains 120, and the planner selects a path. (Take a flat detour). When At that time, path The total cost is , and path When the value is close to 120, the planner tends to choose the correct path. However, the difference in cost is very small.

[0177] The above experimental results show that by adjusting The parameters allow for flexible control over the planner's sensitivity to terrain difficulty. In actual deployment, it is recommended to dynamically adjust these parameters based on specific task requirements and the robot's motion state. Reduced battery life during time-sensitive tasks with ample battery power. To select shorter crossing routes; to improve performance in missions where battery power is low and safety is the priority. To choose a flatter path.

[0178] Experiment Example 4: Autonomous Navigation in Ruins Search and Rescue Environment

[0179] This experimental example uses a quadruped robot dog's autonomous navigation task in an earthquake rubble search and rescue scenario to demonstrate the system's adaptability in extremely complex terrain.

[0180] The environment for search and rescue in ruins is characterized by irregular terrain with numerous obstacles such as rubble, collapsed building components, sloping concrete slabs, and twisted steel bars. The terrain features in this environment are far more complex than those of urban sidewalks, presenting a combination of challenges: high steps (rubble piles), steep slopes (sloping concrete slabs), and high roughness (rubble surfaces). Furthermore, GPS signals in ruins are often blocked or completely unavailable, and LiDAR and visual sensors face significant interference from dust, smoke, and other factors, placing extremely high demands on the robustness of the SLAM system and the anti-interference capabilities of the terrain analysis module. In such extreme environments, the complementary dual-system mechanism of LVI-SAM effectively addresses the degradation problem of a single sensor, ensuring the basic reliability of map building and pose estimation.

[0181] In this experimental example, the physical capability parameters were appropriately relaxed based on the specific requirements of the search and rescue mission: Adjusted to 22 centimeters (search and rescue robot dogs usually have stronger jumping ability). Adjust to 35 degrees. Adjust to 10 cm. Set the grid resolution to [value missing]. Centimeters to accommodate the need for scanning a wider range of terrain.

[0182] In rubble environments, visual semantic segmentation plays a crucial role. The BiSeNet-V2 network was fine-tuned for search and rescue scenarios, adding semantic categories specific to search and rescue, such as "rubble piles," "sloping slabs," and "metal obstacles." The fine-tuning training used a dedicated dataset containing 2000 labeled images of search and rescue scenarios, achieving an average intersection-over-union (mIoU) of 78.3% on the validation set, meeting practical application requirements. Through joint semantic-geometric judgment, the system can correctly identify exposed rebar as impassable dangerous obstacles (even if their geometric height is within the robot dog's traversal capabilities), effectively ensuring the robot dog's safety in hazardous environments.

[0183] During the path planning phase, the terrain penalty coefficient is set to... This invention encourages the robot dog to more actively traverse navigable complex terrain during search and rescue missions to reach the target area as quickly as possible. Test results show that, compared to traditional navigation methods, the system of this invention increases the explorable area of ​​the robot dog in ruins by approximately 60% and shortens the average search path by approximately 25%, significantly improving the efficiency and coverage of search and rescue missions.

[0184] Example 5: Overall Algorithm Performance Analysis and Parameter Sensitivity Experiment

[0185] This experimental example systematically tests and analyzes the overall performance of the LVI-SAM-Trav system, covering three dimensions: SLAM positioning accuracy, terrain analysis accuracy, and path planning efficiency. The sensitivity of key parameters is also experimentally verified.

[0186] Regarding SLAM positioning accuracy, the system recorded the pose estimation errors output by the LVI-SAM fusion framework in three test scenarios: urban sidewalks, indoor staircases, and ruins. In the urban sidewalk scenario (total path length approximately 500 meters), the mean absolute trajectory error (ATE) after loop closure was 0.12 meters, with a maximum error not exceeding 0.35 meters. In the indoor staircase scenario (containing three floors of stairs, total path length approximately 150 meters), the mean ATE was 0.08 meters, with a maximum error not exceeding 0.22 meters, indicating that LVI-SAM factor map fusion has high positioning accuracy in structured environments. In the ruins scenario (total path length approximately 200 meters), due to the highly unstructured environment and significant occlusion, the mean ATE slightly increased to 0.25 meters, but still met the requirements of the terrain analysis module for grid-level (5 cm resolution) positioning accuracy.

[0187] Regarding the accuracy of terrain analysis, manually labeled terrain categories were used as ground truth to evaluate the terrain classification results of the system of this invention. The terrain classification accuracy of the pure geometric analysis method was 82.3%, with the main sources of misclassification being wall edges misidentified as steps (accounting for 47% of the total misclassifications) and the bottom of railings misidentified as walkable ramps (accounting for 31% of the total misclassifications). After introducing visual semantic auxiliary correction, the terrain classification accuracy improved to 96.1%, and the misclassification rate dropped significantly from 17.7% to 3.9%, verifying the effectiveness of the semantic-geometric joint judgment method.

[0188] Regarding path planning efficiency, path planning tests were conducted using both a traditional binary cost map and the TTW cost map of this invention. In urban environments containing steps and gentle slopes, the success rate of path planning using the traditional method was 72% (due to numerous passable areas being misjudged as fatal obstacles, leading to path interruptions), while the success rate of path planning using the method of this invention was 98%. When planning is successful, the average path length generated by the method of this invention is 18% to 25% shorter than that of the traditional method, while the actual path traversal success rate (the proportion of times the robot dog can safely reach the destination along the planned path) reaches over 95%.

[0189] Regarding sensitivity to key parameters, the mesh resolution... Upper limit of step cost Terrain penalty coefficient Parametric scanning experiments were conducted. Experimental results show that the grid resolution... Within a range of 3 to 8 centimeters, the impact on terrain classification accuracy is minimal (change not exceeding 2%), but Excessively large step sizes (over 10 cm) can cause small step features to be smoothed out, reducing classification sensitivity. Upper limit of step cost. The impact of a factor in the range of 0.5 to 0.8 on the path planning results is relatively mild. The recommended default value is used for terrain penalty coefficient. The impact on path selection strategy is most significant, and it is recommended to make targeted adjustments based on specific application scenarios and task priorities.

[0190] The key differences between this invention and conventional solutions are shown in the table below:

[0191]

[0192] This invention innovatively introduces a terrain undulation décor traversability weight (TTW) mechanism based on the LVI-SAM framework. The core innovation of this method lies in replacing the traditional binary obstacle judgment with a continuous value weight function. By fusing geometric analysis of LiDAR point clouds and visual semantic segmentation information, a comprehensive traversability weight between 0 and 1 is calculated for each terrain grid cell, and this weight is introduced into the cost function of path planning for explicit optimization.

[0193] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. For those skilled in the art, the present invention can have various modifications and variations. For example, the cosine soft threshold mapping function can be replaced with other mapping functions with similar smooth transition characteristics, such as the Sigmoid function, piecewise linear function, Gaussian cumulative distribution function (CDF), etc.; the probability or fusion strategy can be replaced with weighted product, weighted summation, fuzzy logic reasoning, or deep learning-based end-to-end fusion methods; the BiSeNet-V2 semantic segmentation network can be replaced with other lightweight or high-precision semantic segmentation models such as DDRNet, PIDNet, and SegFormer. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for autonomous navigation of a legged robot based on terrain passability weight, characterized by, Includes the following steps: (1) Configure the Vision-Inertial Subsystem (VIS): It is equipped with a monocular camera and an inertial measurement unit (IMU) to extract image feature points. It provides short-term motion priors through IMU pre-integration and performs nonlinear optimization by minimizing the joint cost function of visual reprojection error and IMU pre-integration residual within a sliding window to obtain local high-frequency pose estimation. At each key frame, the Vision-Inertial Subsystem encapsulates the relative pose constraints between adjacent key frames obtained by the above nonlinear optimization into a visual inertial odometry factor containing rotation matrix, velocity vector and displacement vector, and passes it to the factor graph fusion module. (2) Configure the lidar-inertial subsystem (LIS): It is equipped with lidar sensors and inertial measurement units to perform voxel downsampling and feature classification on the original point cloud. The point cloud is divided into two categories: edge features and planar features. The weighted sum of the distance residuals from edge feature points to corresponding edge lines and the distance residuals from planar feature points to corresponding planes is minimized by scan-to-map matching. The 6-DOF pose transformation matrix that minimizes the sum of the residuals is solved, and the lidar odometry factor is output. (3) Factor graph fusion: Connect the vision-inertial subsystem and the lidar-inertial subsystem, and model the odometry factor and odometry factor output by the two subsystems into factor nodes in the factor graph. Perform global joint optimization through iSAM2 incremental smoothing and graph building algorithm to output a globally consistent robot state estimate and a three-dimensional point cloud map containing six degrees of freedom pose, three-dimensional velocity and IMU bias of three-dimensional position and three-dimensional attitude. (4) terrain analysis TAM: receiving the three-dimensional point cloud map output by the factor graph fusion step, constructing a 2.5D elevation grid, wherein the elevation grid projects the three-dimensional point cloud into a two-dimensional uniform grid on the horizontal plane, the grid resolution is , the grid row index is denoted as , the column index is denoted as , and each grid cell is denoted as ; extracting three terrain features of step height, local slope and roughness for each grid cell, calculating the corresponding passability sub-weight of each feature through the cosine soft threshold mapping function, and calculating the comprehensive passability weight by using the probability or fusion strategy, and then combining the visual semantic segmentation result to perform semantic-geometric joint correction, and finally outputting the passability cost map; (5) Path planning: Based on the drivability cost map output from the terrain analysis step, the TTW-A* path planning algorithm is used to plan the path in the standard... The algorithm's cost function incorporates terrain accessibility weights as a penalty term, and its cost accumulation formula is as follows: ,in This represents the Euclidean distance between the current node and its parent node. This represents the overall accessibility weight of the grid corresponding to the current node. The terrain penalty coefficient is used to generate the path with the minimum total weighted cost that takes into account terrain décor, where the total weighted cost is defined as the edge cost between all adjacent nodes on the path. The cumulative summation of the values ​​ensures that the legged robot can select the route with the minimum total weighted cost in complex terrain environments containing steps, gentle slopes, and rough ground.

2. The autonomous navigation method for a legged robot based on terrain drivability weights according to claim 1, characterized in that, The specific processing flow of the vision-inertial subsystem extracts sparse image feature points through the BRIEF descriptor, performs inter-frame feature tracking using the KLT optical flow method, and uses the IMU pre-integration factor as a short-term motion constraint to jointly optimize camera pose, landmark position, and IMU bias parameters within a sliding window optimization framework. The BRIEF descriptor generates a fixed-length binary feature vector by comparing the brightness of randomly selected pixel pairs within a local region of the image, which is used to quickly match the same feature points between adjacent frames. The KLT optical flow method is based on the Lucas-Kanade optical flow model, assuming that the neighborhood grayscale of the same feature point between adjacent frames remains consistent, and iteratively solves the sub-pixel displacement of the feature point between adjacent frames by minimizing the template matching residual. The IMU pre-integration integrates all IMU acceleration and angular velocity measurements between two keyframes into relative rotation increments without relying on absolute poses , relative velocity increments , and relative displacement increments , and encapsulates the increments as constraint factors in a factor graph; the sliding window optimization maintains a fixed number of recent keyframes, with a window size of Herbs, within the window, a joint cost function of visual re-projection errors and IMU pre-integration residuals is minimized by Gauss-Newton or Levenberg-Marquardt algorithms, and the oldest keyframe in the window is marginalized when a new keyframe is added to control the computation scale.

3. The autonomous navigation method for a legged robot based on terrain drivability weights according to claim 1, characterized in that, The lidar-inertial subsystem performs curvature analysis on the original point cloud, classifying high-curvature points as edge features and low-curvature points as planar features. Through scan-to-map matching with the global feature map, it minimizes the weighted distance residuals from points to edge lines and from points to planes, and solves for the 6-DOF pose transformation matrix in the current frame that minimizes the sum of these weighted distance residuals. The specific curvature analysis method is as follows: for each point in a single lidar frame scan, the rate of change of distance between that point and its five adjacent points along the scan line direction is calculated as the curvature estimate for that point. Points with curvature values ​​exceeding a set threshold are classified as edge features. Points with curvature values ​​below a set threshold are classified as planar feature points. The specific scan-to-map matching method involves associating the extracted edge and planar feature points from the current frame with the existing edge segments and planes in the global feature map using nearest neighbor relationships. The distance residual of an edge feature point is defined as the point-to-line distance from the point to its corresponding edge segment, and the distance residual of a planar feature point is defined as the point-to-plane distance from the point to its corresponding plane. The 6-DOF pose transformation matrix, which minimizes the weighted sum of the distance residuals from these two types of points, is then iteratively solved using the Levenberg-Marquardt nonlinear optimization algorithm. ,in Let be the pose transformation matrix to be optimized. For edge feature points, For planar feature points, and These are the distance functions from the point to the edge line and from the point to the plane, respectively.

4. The autonomous navigation method for a legged robot based on terrain drivability weights according to claim 1, characterized in that, The terrain analysis step (TAM) performs accessibility analysis on the 3D point cloud, employing a terrain accessibility cost map construction method based on cosine soft thresholding and probability fusion, as detailed below: Step 1: Elevation Grid Construction: Receive the 3D point cloud map data output from the factor map fusion step. The 3D point cloud data must be represented in a gravity-aligned world coordinate system. The axis points towards the zenith and the anti-gravity direction; the horizontal plane is defined as... Coordinate isosurfaces; the 3D point cloud is projected onto a 2D uniform grid on a horizontal plane to construct a 2.5D elevation grid; the grid resolution is r, in meters, and the origin coordinates of the elevation grid are defined as... Input point cloud Input point cloud 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; 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 ignored. , 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: ； 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; Step 3: Local slope feature extraction: For each effective grid cell Take it as the center Average elevation data of the effective grid cells within the window. For grid cells located at elevation grid boundaries, 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) Represents the normal vector of the fitted plane, subscript Representing "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 legged 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 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 has a function value of 0 and a first derivative dw / dx = 0 at x = x free The cosine soft threshold mapping function has a function value of 1 and a first derivative dw / dx = 0 at x = x max The physical meaning of the zero derivative boundary condition is that the weight curve is smoothly cut in and cut out at the two endpoints of "completely passable" and "completely impassable", with a slope of zero, avoiding the emergence of cost mutation 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 the image stream of the visual-inertial subsystem to classify 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 the 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. 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 [0, 0, 1]], where the extrinsic parameters of the camera 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 midpoint to the ray is less than 1 / 2. 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. 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 is impassable; for the grid cells marked as unobserved regions in step one, a cost of 127 (cost space) is assigned. The intermediate value represents the unknown area; thus, a traversability cost map covering the entire elevation grid is generated.

5. The autonomous navigation method for a legged robot based on terrain drivability weights according to claim 4, 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. ; The legged robots include quadrupedal robotic 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.

6. The autonomous navigation method for a legged robot based on terrain drivability weights according to claim 4, characterized in that, The probability or fusion strategy in step six satisfies the following three mathematical properties: when hour, ;when , , When any one of them equals 1, When all three sub-weights are between 0 and 1, Strictly greater than .

7. The autonomous navigation method for a legged robot based on terrain drivability weights according to claim 4, 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; 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, the optimizer is SGD, the learning rate is 0.01, the momentum is 0.9, and the weight decay is... 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.

8. The autonomous navigation method for a legged robot based on terrain drivability weights according to claim 4, characterized in that, It also includes the sliding window point cloud map update steps: maintaining a point cloud map centered on the robot's current position with a radius of [missing information]. 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... 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.

9. The autonomous navigation method for a legged robot based on terrain drivability weights according to claim 1, characterized in that, The TTW-A* path planning algorithm in path planning (5) modifies the cost function based on the standard A* algorithm as follows: ； in For nodes Total estimated cost, From the starting point to the node The cumulative actual cost, For the node Heuristic cost estimation to the destination; The accumulation method has been modified as follows: ； in The cumulative cost of the parent node. This represents the Euclidean distance between the current node and its parent node. This represents the overall accessibility weight of the grid corresponding to the current node. The terrain penalty coefficient; when When the value is large, the planner tends to choose flat routes to avoid terrain obstacles; when When a smaller value is selected, the planner is more inclined to bypass the steps to shorten the journey, thereby achieving an adjustable total weighted cost minimization trade-off between "taking a detour on flat ground" and "going straight through the steps".

10. The autonomous navigation method for a legged robot based on terrain drivability weights according to claim 9, characterized in that: The terrain penalty coefficient The value ranges from 1.0 to 10.0 and is dynamically adjusted according to task requirements and the robot's motion capabilities: when the task prioritizes motion safety, Take the larger value; when path efficiency is prioritized in the task. Take the smaller value; the heuristic cost estimation Use Euclidean distance or diagonal distance to ensure that the heuristic function satisfies the consistency condition.

Images