Bidirectional search path planning method and system based on three-dimensional point cloud terrain evaluation

Abstract

The present application relates to the technical field of autonomous navigation of ground mobile robots, and particularly relates to a bidirectional search path planning method and system based on three-dimensional point cloud terrain evaluation. The method acquires a three-dimensional point cloud map, a path starting point and a path ending point, separates terrain points and obstacle points, establishes a local point cloud neighborhood and fits a local terrain plane, generates a passing cost, filters effective nodes and constructs a search graph, constructs an obstacle distance distribution, extends a forward search end and a reverse search end, forms a candidate connection pair, checks the terrain passability, minimum obstacle distance and steering change of the connection segment, traces back along the parent node to form an initial path, retreats path nodes or inserts control points for a curve segment of a smooth path that does not satisfy a constraint, and outputs a global path. The present application makes terrain constraints act on node filtering, bidirectional connection and smooth path checking, and reduces the situation of path crossing impassable areas and entering obstacle inflation areas.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation technology for ground mobile robots, and in particular to a bidirectional search path planning method and system based on three-dimensional point cloud terrain assessment. Background Technology

[0002] In the field of autonomous navigation technology for ground mobile robots, existing global path planning schemes typically use two-dimensional occupancy grid maps, elevation maps, voxel maps, or three-dimensional point cloud maps to describe the robot's operating environment, and perform path search based on the path start point, path end point, and obstacle distribution. For three-dimensional point cloud maps, downsampling, outlier filtering, terrain point extraction, and local plane fitting are usually performed first. Then, the path cost is constructed based on slope, terrain undulation, or obstacle distance, and a heuristic search algorithm is used to form a sequence of path nodes from the path start point to the path end point.

[0003] In complex 3D terrains such as slopes, stairs, steps, local depressions, and insufficient point cloud coverage, establishing a search space solely based on 2D projection, a single elevation difference, or binary obstacle markers is insufficient to fully reflect the slope, flatness, point cloud sparsity, and edge variations of the local terrain. While some existing solutions generate terrain evaluation results, these results are often used for path cost calculation or terrain category labeling. The lack of continuous constraints between terrain evaluation and search node selection and adjacency relationships makes it easy for the search process to enter areas where the terrain state cannot be confirmed or does not meet the robot's traversal conditions.

[0004] When using bidirectional search, existing solutions typically extend the search front from the path start point and the path end point respectively, and then splice the two paths together when the two search fronts meet or are spatially close. This type of connection method focuses more on the distance relationship between the connecting nodes, and lacks joint verification of the terrain accessibility of the area traversed by the connecting segment, the minimum distance between the connecting segment and obstacles, and the turning changes at the connection point. This can easily lead to situations where the two search fronts spatially meet the connection conditions, but the corresponding connecting segment crosses terrain edges, is close to obstacles, or forms a large directional change.

[0005] The discrete paths obtained through searching typically require node deletion and curve fitting to form smooth paths. Curve fitting can cause paths to deviate from the original path's polygonal range between nodes. In areas with dense obstacles or near the boundaries of passable areas, the smoothed curve may enter areas of obstacle expansion, deviate from the effective coverage of shape nodes, or pass through areas where node-level terrain passability does not meet requirements. Therefore, it is necessary to address the difficulty in continuously connecting node-level terrain passability with bidirectional search connections, connection path verification, and smoothed path verification in 3D point cloud maps under complex 3D terrain. Summary of the Invention

[0006] To address the aforementioned technical problems, this invention provides a bidirectional search path planning method based on three-dimensional point cloud terrain assessment, comprising:

[0007] S100: Obtain a 3D point cloud map, path start point, and path end point; separate terrain points and obstacle points from the 3D point cloud map; and generate a candidate node set and an obstacle point set based on the terrain points.

[0008] S200. Establish a local point cloud neighborhood based on the candidate node set and fit a local terrain plane to generate local terrain features; determine the passage cost based on the local terrain features, use the passage cost to filter effective nodes and establish adjacency relationships, and generate a three-dimensional search graph.

[0009] S300. Based on the three-dimensional search map and obstacle point set, construct the obstacle distance distribution, expand the forward search end and reverse search end from the path start point and path end point, include the passage cost and obstacle distance cost in the cumulative cost, and generate candidate connection pairs.

[0010] S400: Verify the connection path based on the node-level terrain accessibility, minimum obstacle distance, and turning change of the connection segment in the candidate connection pair; if the verification is successful, track the forward parent node and the reverse parent node to generate an initial path point sequence; generate a smooth path based on the initial path point sequence and verify it to obtain curve segments that do not meet the constraints; backtrack the path nodes or insert path nodes based on the curve segments that do not meet the constraints, regenerate and output the three-dimensional global path.

[0011] Furthermore, in S100, robot passage parameters are also obtained, including robot equivalent safety radius, maximum allowable slope, maximum allowable terrain undulation, search step size, terrain passability threshold, and obstacle expansion radius;

[0012] The three-dimensional point cloud map is cropped, voxel downsampled, and statistical outlier filtered out to generate a preprocessed point cloud;

[0013] Based on the normal vector and elevation difference of the preprocessed point cloud, terrain points and obstacle points are separated. A spatial index is constructed based on the terrain points, and candidate terrain nodes are determined according to the search step size.

[0014] Furthermore, in S200, the establishment of the local point cloud neighborhood includes:

[0015] Using candidate terrain nodes as the center, query terrain points from the spatial index according to the local neighborhood scale;

[0016] Point cloud missing markers are generated based on the number of neighboring points in the local point cloud neighborhood and the point cloud coverage.

[0017] Candidate terrain nodes with missing point cloud markers are marked as impassable points, and local terrain planes are fitted to the remaining candidate terrain nodes.

[0018] The process of establishing a local point cloud neighborhood based on a candidate node set and fitting a local terrain plane includes:

[0019] The local neighborhood scale is configured in association with the robot's footprint size and search step size, and the neighborhood range covers the terrain area that the robot may come into contact with when it is at that node.

[0020] The horizontal projection area of ​​the local neighborhood is divided into multiple directional sub-regions. The number of directional sub-regions containing effective shape points is counted and the ratio to the total number of directional sub-regions is calculated to obtain the point cloud coverage rate. When the number of neighborhood points is lower than the threshold for the number of neighborhood points or the point cloud coverage rate is lower than the threshold for the point cloud coverage rate, the corresponding candidate terrain nodes are marked with point cloud missing markers and marked as impassable points.

[0021] For candidate terrain nodes that are not marked as impassable, calculate the centroid of each terrain point in the local point cloud neighborhood, form a data matrix with the coordinates of each terrain point relative to the centroid, obtain the normal vector of the local terrain plane through singular value decomposition, and use the centroid as a reference point on the local terrain plane.

[0022] Furthermore, the generation of local terrain features includes:

[0023] The slope is calculated based on the angle between the normal vector of the local terrain plane and the Z-axis of the world coordinate system; the slope of a horizontal ground is close to zero.

[0024] Flatness is calculated based on the root mean square of the vertical distance from the terrain point in the local point cloud neighborhood to the local terrain plane, and the root mean square is used as the terrain undulation value.

[0025] Point cloud sparsity is calculated by weighting the ratio of the number of directions without point cloud coverage to the total number of directional sub-regions and the void ratio.

[0026] Edge features are extracted based on the elevation difference between high-order and low-order quantiles in the local point cloud neighborhood.

[0027] Slope, flatness, point cloud sparsity, and edge features are normalized according to the maximum allowable slope, the flatness reference value corresponding to the maximum allowable terrain undulation, the sparsity upper limit of 1, and the maximum allowable terrain undulation, respectively. The four normalized features are then weighted and fused according to preset weights to form the passage cost corresponding to the candidate terrain node. The weights are configured according to the robot's structural form and passage capability.

[0028] The step of using the passage cost to filter valid nodes and establish adjacency relationships includes:

[0029] When the passage cost of a candidate terrain node exceeds the terrain passability threshold, the corresponding candidate terrain node is marked as an impassable point, and the remaining candidate terrain nodes are determined as valid nodes. The passage cost maintains a correspondence with the valid nodes after the node screening is completed, and is used as a terrain component of the cumulative cost in subsequent steps.

[0030] For the current valid node, query other valid nodes whose three-dimensional Euclidean distance is within the allowed range of the search step size from the spatial index; for each node to be connected, compare the elevation difference between the two nodes and the angle between the local terrain plane normal vectors. If the elevation difference exceeds the maximum allowed terrain undulation or the angle between the normal vectors exceeds the terrain continuity threshold, do not establish a search adjacent edge.

[0031] For the straight line connection between two valid nodes, set connection sampling points with a sampling interval no greater than the search step size. Query the passage cost of valid nodes near each connection sampling point. When a connection sampling point cannot be mapped to a valid node or its passage cost exceeds the terrain passability threshold, do not establish a search adjacent edge.

[0032] Furthermore, the determination and invocation of the passage cost includes:

[0033] Slope, flatness, point cloud sparsity and edge features are normalized and weighted fused to generate the passage cost corresponding to candidate terrain nodes;

[0034] Candidate terrain nodes whose passage cost exceeds the terrain passability threshold are marked as impassable points, and the remaining candidate terrain nodes are determined as valid nodes;

[0035] Adjacency relationships are established based on the three-dimensional Euclidean distance between valid nodes and terrain continuity, and the passage cost corresponding to the valid nodes is written into the cumulative cost.

[0036] Furthermore, in S300, the generation of the obstacle distance cost and cumulative cost includes:

[0037] Query the distance from a valid node to the nearest obstacle point;

[0038] When the distance to the obstacle is not greater than the robot's equivalent safe radius, the corresponding valid node will be deleted from the 3D search graph;

[0039] When the obstacle distance is greater than the robot's equivalent safety radius and less than the obstacle's expansion radius, a continuously decaying obstacle distance cost is generated based on the obstacle distance.

[0040] The geometric movement cost is generated based on the three-dimensional Euclidean distance between adjacent valid nodes, and the turning penalty is generated based on the turning angle between the parent node direction and the candidate node direction. The geometric movement cost, passage cost, obstacle distance cost and turning penalty are included in the cumulative cost.

[0041] Furthermore, the expansion of the forward search end and the reverse search end includes:

[0042] Establish a forward open node set and a forward closed node set based on the valid nodes corresponding to the starting point of the path, and establish a reverse open node set and a reverse closed node set based on the valid nodes corresponding to the ending point of the path;

[0043] A composite evaluation value is generated based on the cumulative cost and the heuristic cost. Nodes to be expanded are selected from the forward open node set and the reverse open node set according to the composite evaluation value.

[0044] The heuristic cost is generated by performing an exponential function mapping on the 3D distance from the node to be expanded to the target node.

[0045] Record the parent node of each node to be expanded, and generate candidate connection pairs based on the spatial distance between the forward search end and the reverse search end.

[0046] Furthermore, in S400, the connection path verification includes:

[0047] When the spatial distance between a node in the forward search end and a node in the reverse search end is not greater than the connection distance threshold, multiple verification sampling points are set on the corresponding connection segment.

[0048] Query the node-level terrain accessibility and obstacle distance corresponding to each verification sampling point, and calculate the turning angle between the forward path direction, the connecting segment direction, and the reverse path direction;

[0049] When the node-level terrain accessibility corresponding to each verification sampling point does not exceed the terrain accessibility threshold, the obstacle distance is not less than the robot's equivalent safety radius, and the turning angle does not exceed the turning angle threshold, a connection path verification result is generated.

[0050] If a connection segment fails the verification, continue to expand the forward or reverse search end; if a connection segment passes the verification, trace the forward and reverse parent nodes respectively and concatenate them to generate the initial path point sequence.

[0051] Furthermore, the generation and verification of the smooth path includes:

[0052] Perform node-level terrain accessibility and obstacle distance verification on the connecting segments between adjacent non-contiguous nodes in the initial path point sequence, delete the intermediate nodes between adjacent non-contiguous nodes that pass the verification, and generate a simplified path node sequence.

[0053] Using the simplified path node sequence as path nodes, a cubic B-spline curve is generated to obtain a smooth path;

[0054] Multiple path sampling points are set on the smooth path, and curve segments that do not meet the constraints are determined based on the node-level terrain accessibility and obstacle distance corresponding to each path sampling point.

[0055] Restore the initial path point corresponding to the curve segment that does not meet the constraint, or insert a path node into the curve segment that does not meet the constraint to regenerate a smooth path;

[0056] When the regenerated smooth path passes the path feasibility test, the three-dimensional global path is output; when the number of path corrections reaches the correction threshold and the smooth path fails the path feasibility test, the initial path point sequence is output.

[0057] Furthermore, a bidirectional search path planning system based on three-dimensional point cloud terrain assessment, applied to any of the methods described above, includes: a point cloud preprocessing module, a terrain assessment and search map construction module, a bidirectional search module, a connection path verification and initial path generation module, and a path smoothing and detection module.

[0058] The key innovations of this invention include:

[0059] (1) The slope, flatness, point cloud sparsity and edge features corresponding to the candidate terrain nodes are integrated into the passage cost, and the same passage cost simultaneously constrains the effective node selection, adjacency relationship establishment and bidirectional search cumulative cost calculation, so that the terrain assessment results run through the process of three-dimensional search map construction and search front expansion.

[0060] (2) After the candidate connection pair is formed between the forward search end and the reverse search end, the node-level terrain accessibility of the area through which the connection segment passes, the minimum obstacle distance of the connection segment, and the turning changes at both ends of the connection segment are jointly called to verify the connection path; after the connection path verification is passed, the forward parent node and the reverse parent node are tracked to form an initial path point sequence; if the connection path verification fails, the forward search end or the reverse search end is expanded.

[0061] (3) After forming a smooth path based on the initial path point sequence, the feasibility of the smooth path is tested by using the passage cost and obstacle distance distribution formed in the search phase; the path nodes are backed up according to the correspondence between the curve segments that do not meet the constraints and the initial path point sequence, or the path nodes are inserted into the curve segments that do not meet the constraints, and the smooth path is regenerated.

[0062] The following are its main beneficial effects:

[0063] (1) The passage cost is simultaneously involved in the admission of search nodes, the establishment of adjacency relationships and the calculation of cumulative costs, so that the slope, local undulation, missing point cloud coverage and terrain edge form continuous constraints on the search space and node expansion order, reducing the situation where the search path enters the area where the terrain state cannot be confirmed or does not meet the robot passage conditions.

[0064] (2) After the candidate connection pair is formed, the forward path and the reverse path are not directly spliced. Instead, the connection segment is checked by combining the node-level terrain accessibility, minimum obstacle distance and turning change, so as to reduce the situation where the connection segment crosses the impassable terrain, enters the robot's equivalent safe radius, or forms a large directional change at the splicing position.

[0065] (3) The smooth path continues to accept the same terrain accessibility and obstacle distance constraints as in the search phase, and backs up or inserts path nodes based on curve segments that do not meet the constraints, reducing the path tangent caused by curve fitting, deviating from the effective shape node coverage range and entering the obstacle expansion area. Attached Figure Description

[0066] Figure 1 A flowchart illustrating the bidirectional search path planning method based on 3D point cloud terrain assessment provided in this application embodiment;

[0067] Figure 2 The structural block diagram of the bidirectional search path planning system based on three-dimensional point cloud terrain assessment provided in the embodiments of this application is shown. Detailed Implementation

[0068] Example 1: Refer to Figure 1 This is a flowchart illustrating a bidirectional search path planning method based on three-dimensional point cloud terrain assessment provided in an embodiment of the present invention. The process may include at least steps S100-S400:

[0069] S100: Obtain a 3D point cloud map, path start point, and path end point; separate terrain points and obstacle points from the 3D point cloud map; and generate a candidate node set and an obstacle point set based on the terrain points.

[0070] S200. Establish a local point cloud neighborhood based on the candidate node set and fit a local terrain plane to generate local terrain features; determine the passage cost based on the local terrain features, use the passage cost to filter effective nodes and establish adjacency relationships, and generate a three-dimensional search graph.

[0071] S300. Based on the three-dimensional search map and obstacle point set, construct the obstacle distance distribution, expand the forward search end and reverse search end from the path start point and path end point, include the passage cost and obstacle distance cost in the cumulative cost, and generate candidate connection pairs.

[0072] S400: Verify the connection path based on the node-level terrain accessibility, minimum obstacle distance, and turning change of the candidate connection segments; if the verification passes, track the forward and reverse parent nodes to generate an initial path point sequence; generate a smooth path based on the initial path point sequence and verify it to obtain curve segments that do not meet the constraints; backtrack the path nodes or insert path nodes based on the curve segments that do not meet the constraints, regenerate and output the three-dimensional global path.

[0073] S100. Obtain a 3D point cloud map, path start point, and path end point. Separate terrain points and obstacle points from the 3D point cloud map. Generate a candidate node set and an obstacle point set based on the terrain points.

[0074] This embodiment applies to global path planning for a ground mobile robot in a static 3D environment. The ground mobile robot can be a quadruped, wheeled, tracked, or wheel-legged robot. The path planning process is executed by a global path planner in the robot's onboard computer. This global path planner includes a point cloud preprocessing module, a terrain assessment and search map construction module, a bidirectional search module, a path generation module, and a path verification module. The 3D point cloud map is pre-built by a LiDAR mapping device, a depth camera mapping device, or an external surveying device, and uniformly converted to the world coordinate system used for robot navigation. The path start point is the robot's current pose position in the world coordinate system, and the path end point is given by the navigation task.

[0075] Upon receiving the global path planning task, the point cloud preprocessing module reads the 3D point cloud map, path start point, path end point, and robot mobility parameters. These robot mobility parameters include the robot's equivalent safe radius, maximum allowable slope, maximum allowable terrain undulation, search step size, terrain mobility threshold, and obstacle expansion radius. The robot's equivalent safe radius is configured based on the robot's horizontal projection range, positioning error range, and body sway range during movement; the search step size constrains the spatial interval between adjacent candidate terrain nodes; and the maximum allowable slope and maximum allowable terrain undulation are used for subsequent terrain point selection and node mobility calculation.

[0076] After loading the 3D point cloud map, the point cloud preprocessing module first checks the coordinate system of the point cloud map against the coordinate systems of the path start and end points. If the coordinate systems are inconsistent, a coordinate transformation is performed on the path start and end points according to the pre-defined coordinate transformation relationship; if no valid coordinate transformation relationship is obtained, the current path planning task stops and a coordinate system inconsistency indicator is generated. After the coordinate system check is completed, the 3D point cloud map is clipped according to the path start, end points, and the enclosing area between them. The clipping range is expanded outward by a preset distance along each coordinate direction so that the subsequent search area includes the surrounding space of the start and end points. If the path start or end point is outside the clipped point cloud range, the clipping range is expanded again; if the expanded range still does not include the corresponding position, the position is marked as an invalid path endpoint.

[0077] The cropped point cloud first undergoes voxel downsampling. The point cloud preprocessing module divides the 3D space into multiple voxel units, calculates the centroid of points falling into the same voxel unit, and replaces the original points in that voxel unit with the centroid. The voxel size is configured according to the original resolution of the point cloud and the search step size, and the voxel size is no larger than the search step size, so that the downsampled point cloud still retains the edges of steps, slope changes, and obstacle outlines. After voxel downsampling, the neighboring points of each point are queried and the mean distance of the neighboring points is calculated; when the mean distance of a point's neighboring points exceeds the outlier threshold corresponding to the global neighboring point distance distribution, the point is filtered out as an outlier, forming the preprocessed point cloud.

[0078] In one implementation, the preprocessed point cloud uses a K-dimensional tree (Kd-Tree) to construct a spatial index. During construction, the three-dimensional coordinates of each point are used as the index key, and the point set is recursively partitioned according to the spatial coordinates. The spatial index is jointly invoked by terrain point separation in S100, local point cloud neighborhood query in S200, and nearest obstacle point query in S300, avoiding traversing the entire point cloud for each node.

[0079] The separation of terrain points from obstacle points is performed based on the normal vectors and elevation differences of each point in the preprocessed point cloud. The point cloud preprocessing module queries the local neighborhood centered on the current point, calculates the centroid and covariance matrix of the neighboring points, and uses the eigenvector corresponding to the smallest eigenvalue of the covariance matrix as the normal vector of the current point. The direction of the normal vector is uniformly adjusted so that its Z-axis component in the world coordinate system is not less than zero, avoiding the appearance of normal vectors with opposite directions on the same terrain surface. Subsequently, the angle between the normal vector and the Z-axis of the world coordinate system is calculated, and the elevation difference of the current point relative to the local reference surface in the neighborhood is statistically analyzed.

[0080] When the angle of the normal vector of the current point is within the allowable range of the terrain surface, and the elevation difference between the current point and the neighboring local reference surface does not exceed the terrain point separation threshold, the current point is added to the terrain point set. Points whose normal vectors are close to the horizontal direction, whose local elevations change abruptly, or whose points are located above the terrain surface are added to the obstacle point set. In a staircase environment, points on the stair treads can be added to the terrain point set, while points on the staircase elevations are added to the obstacle point set according to their normal vector directions. In a slope environment, whether slope points are added to the terrain point set also depends on the robot's maximum allowable slope; slope points exceeding the maximum allowable slope are retained in the obstacle point set or the impassable point set.

[0081] For points whose normal vectors cannot be stably calculated, the point cloud preprocessing module checks the number of its neighboring points. If the number of neighboring points is insufficient, the point is not directly added as a terrain point using interpolation, but is instead marked as a point with insufficient point cloud coverage. If the number of neighboring points meets the calculation conditions but the change in the normal vector exceeds the normal vector consistency threshold, the point is classified into the terrain edge region, and the S200 subsequently calculates its passage cost based on the point cloud sparsity and edge features.

[0082] After the terrain point set is formed, the point cloud preprocessing module performs spatial sampling of the terrain points according to the search step size. Specifically, node sampling units corresponding to the search step size are established on the horizontal projection plane of the world coordinate system. Within each sampling unit, terrain points with effective elevations that are close to the center of the unit are selected as candidate terrain nodes. For cases where there are multiple elevation layers at the same horizontal location, candidate terrain nodes corresponding to different elevation layers are retained to avoid merging stairs, platforms, and upper and lower level structures after horizontal projection.

[0083] The path start and end points are respectively queried using spatial indexes to find the nearest candidate terrain nodes. If the distance between the nearest candidate terrain node and its corresponding path endpoint is not greater than the endpoint mapping radius, it is recorded as the start node and end node, respectively. If the distance between the nearest candidate terrain node and its corresponding path endpoint exceeds the endpoint mapping radius, the query range is expanded and the elevation difference and obstacle distance of the candidate terrain nodes within the query range are checked. If no candidate terrain node that meets the robot's passage parameters is found, an invalid start point or invalid end point flag is generated, and the current path planning task does not proceed to S200.

[0084] At the end of S100, a candidate node set, an obstacle point set, a starting node, an ending node, and a point cloud spatial index are generated. Each node in the candidate node set has a node identifier and three-dimensional coordinates, and the obstacle point set retains the three-dimensional coordinates of the obstacle points. The candidate node set and the point cloud spatial index are sent to the terrain assessment and search map construction module. The obstacle point set is used in S300 to construct the obstacle distance distribution.

[0085] S200. Based on the candidate node set, establish a local point cloud neighborhood and fit a local terrain plane to generate local terrain features; determine the passage cost based on the local terrain features, use the passage cost to filter effective nodes and establish adjacency relationships, and generate a three-dimensional search graph:

[0086] After receiving the candidate node set and point cloud spatial index formed by S100, the terrain assessment and search map construction module processes the candidate terrain nodes one by one. For each candidate terrain node, the local point cloud neighborhood is queried from the terrain point set according to the local neighborhood scale, with the three-dimensional coordinates of the node as the neighborhood center. The local neighborhood scale is set according to the robot footprint size and search step size, and the neighborhood range covers the terrain area that the robot may come into contact with when it is at the node. If the local neighborhood scale is too small, the terrain features will be affected by the noise of a single point cloud, and if the local neighborhood scale is too large, adjacent steps or adjacent slopes will be merged. Therefore, this implementation configures the local neighborhood scale in association with the robot footprint size.

[0087] After the local point cloud neighborhood is formed, the number of neighboring points and the point cloud coverage are checked first. The number of neighboring points refers to the number of terrain points retained within the local neighborhood; the point cloud coverage reflects the completeness of the point cloud distribution in the horizontal direction of the local neighborhood. In one implementation, the horizontal projection area of ​​the local neighborhood is divided into multiple directional sub-regions centered on the candidate terrain node. The number of directional sub-regions containing effective terrain points is counted, and the ratio of this number to the total number of directional sub-regions is calculated to obtain the point cloud coverage.

[0088] When the number of neighboring points is below a threshold, or the point cloud coverage is below a threshold, the corresponding candidate terrain nodes are marked with a missing point cloud marker. Candidate terrain nodes with missing point cloud markers do not participate in local terrain plane fitting and are marked as impassable points. This processing applies to local point cloud missingness caused by laser occlusion, reflection loss, terrain holes, or map boundaries; when the local terrain state cannot be confirmed by the existing point cloud, it is not added to the search space as ordinary flat land.

[0089] For candidate terrain nodes not marked as impassable, the terrain assessment and search map construction module fits a local terrain plane. Specifically, the centroid of each terrain point in the local point cloud neighborhood is first calculated, and then the coordinates of each terrain point relative to the centroid are used to form a data matrix. The principal direction of the data matrix is ​​obtained through singular value decomposition. The direction corresponding to the minimum singular value is used as the normal vector of the local terrain plane, and the centroid is used as a reference point on the local terrain plane. In another implementation, the covariance matrix of the local point cloud neighborhood is eigenvalued, and the eigenvector corresponding to the minimum eigenvalue is used as the normal vector of the local terrain plane.

[0090] After the local terrain plane fitting is completed, the distance residuals from neighboring points to the local terrain plane are calculated. When the root mean square of the distance residual exceeds the plane fitting residual threshold, it indicates that multiple terrain surfaces, step edges, or abrupt height changes may exist simultaneously in the current neighborhood. This candidate terrain node is not directly deleted; instead, it retains a large flatness or edge feature in the local terrain characteristics, allowing it to obtain corresponding value in subsequent drivability cost calculations. If the distribution of neighboring points degenerates, causing the local terrain plane normal vector to lose its stable direction, the corresponding candidate terrain node is marked as an impassable point.

[0091] Local terrain features include slope, flatness, point cloud sparsity, and edge features. Slope is calculated based on the angle between the local terrain plane's normal vector and the world coordinate system's Z-axis. When the normal vector is n and its Z-axis component is nz, the slope can be calculated using the inverse cosine relationship of the angle between the normal vector and the Z-axis; the normal vector of a horizontal surface is close to the Z-axis, and its slope is close to zero. The normal vector is normalized before calculation, and its Z-axis component with unified direction is used to avoid inconsistencies in slope values ​​caused by the normal vector's orientation.

[0092] Flatness is calculated based on the dispersion of terrain points in the local point cloud neighborhood to the local terrain plane. In one embodiment, the vertical distance from each terrain point to the local terrain plane is calculated, and the root mean square of the vertical distance is calculated. This root mean square is used as the terrain undulation value corresponding to the flatness. The more concentrated the terrain points are on the fitted plane, the smaller the terrain undulation value; the more dispersed the terrain points are at step boundaries, in gravel areas, or on uneven ground, the larger the terrain undulation value. The flatness described in the specification uses a numerical value representing the degree of unevenness; a larger value indicates a greater degree of deviation of the local terrain from the fitted plane.

[0093] Point cloud sparsity is calculated jointly by the number of directions without point cloud coverage and the void ratio. The terrain assessment and search map construction module divides the local point cloud neighborhood into multiple directional sub-regions. Directional sub-regions that do not contain effective shape points are denoted as directions without point cloud coverage, and the ratio between the number of directions without point cloud coverage and the total number of directional sub-regions is calculated. Simultaneously, the void ratio is calculated based on the relationship between the theoretical coverage area of ​​the local neighborhood and the area occupied by effective shape points. The ratio and void ratio are then weighted to form the point cloud sparsity. Point cloud sparsity is not used to fill in missing points, but rather participates in the passage cost as a measure of the uncertainty of the node's terrain state.

[0094] Edge features are extracted based on the elevation difference between the maximum and minimum elevations in the local point cloud neighborhood. To avoid residual outliers directly determining the maximum or minimum elevation, in one implementation, the neighborhood terrain points are first sorted by elevation, and the elevation difference is calculated by selecting high and low quantile points respectively. When the elevation difference exceeds the robot's maximum permissible terrain undulation, the corresponding candidate terrain node is marked as an impassable point; when the elevation difference does not exceed the maximum permissible terrain undulation, the normalized elevation difference is used as an edge feature in subsequent calculations.

[0095] Since slope, flatness, point cloud sparsity, and edge features have different numerical ranges, the terrain assessment and search map construction module normalizes them according to the reference upper limit of the corresponding features. Slope uses the maximum permissible slope as the normalization reference, flatness uses the planar dispersion corresponding to the maximum permissible terrain undulation as the normalization reference, point cloud sparsity is itself limited to between zero and one, and edge features use the maximum permissible terrain undulation as the normalization reference. Normalization results exceeding one are treated as one, and results below zero are treated as zero.

[0096] Multiple normalized terrain features are weighted and fused according to preset weights to form the travel cost corresponding to the candidate terrain node. Each weight is a non-negative number, and the sum of the weights is one. The specific weights are configured according to the robot's structure and travel capability. For quadruped robots, which are more sensitive to local voids and terrain edges, point cloud sparsity and edge features can be assigned larger weights; for wheeled robots, which are more sensitive to slope and ground undulation, slope and flatness can be assigned larger weights. The larger the travel cost value, the greater the deviation between the region corresponding to the candidate terrain node and the robot's travel parameters.

[0097] When the terrain traversability cost exceeds the terrain traversability threshold, the corresponding candidate terrain node is marked as an impassable point; the remaining candidate terrain nodes are determined as valid nodes. The traversability cost is not discarded after node screening but maintains a correspondence with the valid nodes and is included as a component of the cumulative cost in S300. Therefore, the same terrain assessment result both changes the set of nodes allowed to participate in the search in the 3D search graph and participates in the calculation of path costs between valid nodes.

[0098] After the effective nodes are selected, the terrain assessment and search map construction module establishes adjacency relationships. For the current effective node, other effective nodes whose 3D Euclidean distance is within the allowable search step size are queried from the spatial index. For each node to be connected, the elevation difference between the two nodes, the angle between the local terrain plane normal vectors, and the terrain traversability of the area traversed by the connecting segment between the nodes are compared. If the elevation difference exceeds the maximum allowable terrain undulation, the angle between the normal vectors exceeds the terrain continuity threshold, or the connecting segment passes through an impassable area, no search adjacency edge is established.

[0099] In one implementation, connection sampling points are set for straight-line connection segments between two valid nodes at sampling intervals no greater than the search step size, and valid nodes near each connection sampling point are queried. When a connection sampling point cannot be mapped to a valid node, or its corresponding passage cost exceeds the terrain passability threshold, the connection segment is identified as a terrain discontinuity. A bidirectional search adjacency edge is established between two valid nodes that satisfy the three-dimensional distance and terrain continuity conditions.

[0100] The 3D search graph generated by S200 includes valid nodes, their 3D coordinates, travel costs, and adjacency relationships. If the starting or ending node generated by S100 is marked as impassable in this step, a new search is conducted within its vicinity for valid nodes whose travel costs do not exceed the terrain's passability threshold, and a new starting or ending node is established. If no corresponding valid node exists within its vicinity, the current path planning task identifies the path endpoints as impassable. The 3D search graph, starting node, ending node, and travel costs are then sent to the bidirectional search module.

[0101] In one specific embodiment:

[0102] In the S200 terrain accessibility assessment and search map construction, the system first receives a candidate node set, point cloud spatial index, terrain point set, and robot access parameters from the S100 system. For each candidate terrain node, using its 3D coordinates as the neighborhood center, and based on the local neighborhood scale configured according to the robot footprint size and search step size, the system queries the local point cloud neighborhood from the terrain point set to obtain the neighborhood terrain point set. ,in The number of neighboring points. Centered on the position of the candidate terrain node on the horizontal projection plane, the horizontal projection area of ​​the local neighborhood is divided into equal parts. For each directional sub-region, count the number of sub-regions containing at least one terrain point. Calculate the point cloud coverage using formula ① Formula ① is as follows:

[0103]

[0104] in:

[0105] Point cloud coverage, dimensionless (range 0-1). Calculated from the proportion of directional sub-regions effectively covered by points in the local point cloud neighborhood.

[0106] : The number of directional sub-regions containing at least one valid shape point, dimensionless (integer). It is calculated by comparing the horizontal projected coordinates of each shape point in the local point cloud neighborhood with the boundaries of the preset directional sub-regions.

[0107] : The total number of directional sub-regions divided by the horizontal projection region of the local neighborhood, dimensionless (integer). Preset by the system, for example 8, 12 or 16.

[0108] Simple numerical example: Let Statistics show that ,but This indicates that terrain points exist in 62.5% of the directions. When Below the threshold for the number of neighboring points or When the point cloud coverage is below the threshold, a missing point cloud marker is generated for the candidate terrain node, and it is marked as an impassable point, excluding it from subsequent plane fitting. For nodes not marked as impassable, the local terrain plane is further fitted: Calculate... center of mass The coordinates of each point relative to the centroid are used to form a data matrix. ,right Perform singular value decomposition The right singular vector corresponding to the minimum singular value is the normal vector of the local terrain plane. This process is equivalent to solving the covariance matrix. The eigenvector corresponding to the smallest eigenvalue is shown in Formula ②:

[0109]

[0110] in:

[0111] : A 3×3 covariance matrix, with dimensions of length squared (m²). It is obtained by summing the deviations of each point in the local point cloud neighborhood from the centroid and dividing by the number of points.

[0112] : The unit normal vector of the local terrain plane, dimensionless (3D vector). is the eigenvector corresponding to the smallest eigenvalue of the covariance matrix, with its direction uniformly adjusted so that the Z-axis component is non-negative.

[0113] Covariance matrix The smallest eigenvalue, with dimensions of length squared (m²), reflects the degree of dispersion of the local point cloud along the normal vector direction.

[0114] : The total number of terrain points within the local point cloud neighborhood, dimensionless (positive integer). This is the result of a local neighborhood query provided by the point cloud preprocessing module.

[0115] : The first in the local point cloud neighborhood The data source is a set of three-dimensional coordinate vectors of terrain points, with the dimension being length (m).

[0116] The centroid (geometric center) of all terrain points in the local point cloud neighborhood, with the dimension of length (m). The calculation formula is as follows: .

[0117] : Data matrix, size , No. Behavior Its dimension is length (m). It is used for singular value decomposition.

[0118] : Left singular matrix, size An orthogonal matrix, dimensionless.

[0119] : Singular value diagonal matrix, size The diagonal elements are singular values, and their dimension is length (m).

[0120] The transpose of a right singular matrix, with a size of An orthogonal matrix, dimensionless. Its last column corresponds to the right singular vector of the minimum singular value. .

[0121] Matrix transpose operator.

[0122] Simple numerical example: Suppose the coordinates (in meters) of three points in a certain neighborhood are (0,0,0), (1,0,0.1), and (0,1,0.05), calculate... ,structure Later obtained via SVD , This indicates that the plane is nearly horizontal. (Normal vector) With center of mass The local terrain plane equation is determined jointly. If the covariance matrix cannot be stably solved due to collinearity or degradation of neighboring points (e.g., the ratio of the smallest eigenvalue to the second smallest eigenvalue is less than a threshold), the node is marked as impassable. This section outputs the coverage identifier and local terrain plane parameters for each candidate terrain node. The missing point cloud markers are directly used by the subsequent local terrain feature extraction steps of S200.

[0123] Continuing from the previously obtained local terrain plane normal vector With center of mass and local point cloud neighborhood Calculate slope, flatness, point cloud sparsity, and edge features in the terrain feature group node by node. Slope Defined as the local terrain plane normal vector and the world coordinate system Z-axis unit vector. The included angle is calculated using formula ③:

[0124]

[0125] in:

[0126] : Slope angle, measured in radians (rad), can be converted to degrees. Represents the angle between the local terrain plane normal vector and the Z-axis of the world coordinate system, with values ​​ranging from 0 to π / 2.

[0127] Same as formula ②, unit normal vector, dimensionless.

[0128] The unit vector along the Z-axis of the world coordinate system is defined as follows: , dimensionless.

[0129] Normal vector The Z-axis component is dimensionless and ranges from -1 to 1. Its absolute value is used to calculate the included angle.

[0130] Vector dot product operator.

[0131] : Vector Euclidean norm (magnitude).

[0132] : Inverse cosine function, returns the value in radians.

[0133] Simple numerical example: From formula ②, we get ,but , Radius ≈ 2.56°. Flatness This reflects the degree of dispersion of the local terrain point cloud relative to the fitted plane, and calculates the signed distance from each point to the plane. Then calculate the root mean square, as shown in formula ④:

[0134]

[0135] in:

[0136] Flatness (root mean square of terrain undulation), measured in length (m). It reflects the degree of dispersion of the local point cloud relative to the fitted plane.

[0137] Same as formula ②, the total number of terrain points in the neighborhood of the local point cloud.

[0138] : No. The signed vertical distance from a topographic point to the local topographic plane, measured in length (m). A positive value indicates that the point is on the side pointed to by the plane's normal vector.

[0139] : Same as formula ②, No. The three-dimensional coordinate vector of a terrain point.

[0140] Same as formula ②, local point cloud centroid.

[0141] Same as formula ②, local terrain plane unit normal vector.

[0142] Simple numerical example: Using the same three points as in the previous example, calculate the distance between each point: the distance from point (0,0,0) is... Rice, point (1,0,0.1) The meter, the point (0,1,0.05). Rice, then rice. when When the flatness of the residual exceeds the plane fitting threshold, the node retains a large flatness in subsequent cost calculations instead of being directly deleted. Point cloud sparsity Weighted calculation based on directional coverage loss and hole rate: the local neighborhood horizontal projection is divided into... Each sub-region, denoted by the number of directions without point cloud coverage. void ratio The ratio of the area covered by the local neighborhood theory to the area occupied by the effective points is calculated for... and Assign weights respectively ( ),have to Edge features Range extraction based on local neighborhood elevation: Elevation of each point Sort by high quantile (e.g., 95th percentile) elevation. and lower quantiles (e.g., 5th percentile) elevation Calculate elevation difference ,like If the terrain undulation exceeds the maximum allowable level, mark it as impassable; otherwise, use the normalized version. As .slope Flatness Point cloud sparsity Edge features The numerical ranges differ, depending on the robot's maximum permissible slope. Reference value for flatness corresponding to the maximum permissible terrain undulation (Can be taken as 0.5 times the maximum allowable terrain undulation or an empirical value), sparsity limit 1, maximum allowable terrain undulation Normalization is performed to obtain Then, the weighted fusion is calculated according to formula ⑤ to obtain the passage cost. :

[0143]

[0144] in:

[0145] : Travel cost, dimensionless. The larger the value, the greater the deviation between the region corresponding to the candidate terrain node and the robot's travel parameters.

[0146] : The fusion weights of the four terrain features, dimensionless, non-negative, and summing to 1. Configured according to the robot type (quadruped, wheeled, etc.).

[0147] Normalized slope, dimensionless. The calculation formula is as follows: ,in From formula ③, This represents the maximum permissible slope for the robot.

[0148] Flatness after normalization, dimensionless. The calculation formula is as follows: ,in From formula ④, This is a reference value for the degree of planar dispersion corresponding to the maximum permissible terrain undulation.

[0149] : Normalized point cloud sparsity, dimensionless. Directly taken. It itself (limited to between 0 and 1).

[0150] : Normalized edge features, dimensionless. The calculation formula is as follows: ,in This represents the elevation difference between high and low quantiles in a local neighborhood. The maximum allowable terrain undulation for the robot.

[0151] The robot's maximum permissible slope, measured in radians or degrees, is derived from the robot's travel parameters.

[0152] : Flatness normalized reference value, dimensionless length (m), obtained by mapping the maximum permissible topographic relief (e.g., taking half of the maximum permissible topographic relief or an empirical value).

[0153] : Maximum permissible terrain undulation for the robot, in units of length (m), derived from robot travel parameters.

[0154] Point cloud sparsity, dimensionless, 0–1. Calculated by weighting the proportion of directions without point cloud coverage and the void ratio.

[0155] Elevation difference between high and low quantiles within a local neighborhood, in units of length (m). Calculated after sorting and removing outliers.

[0156] The minimum value function limits the normalized result to no more than 1.

[0157] Simple numerical example: Let (0.349 radians) but ; rice, Mize ;set up , , have to ; rice, Mize Take the weights of the quadruped robot ,but .when Points exceeding the terrain passability threshold (e.g., 0.8) are marked as impassable; otherwise, they are considered valid nodes. The value is preserved and bound to the node for cumulative cost calculation in S300. This section outputs the passage cost for each candidate terrain node. The valid node identifiers and local terrain plane parameters are used by the adjacency relationship construction step of S200 and the bidirectional search module of S300.

[0158] After obtaining the set of valid nodes and their passage costs, adjacency relationships are established to form a 3D search graph. For each valid node, the 3D Euclidean distance to it within the search step is queried using the point cloud spatial index. Other valid nodes within the allowed range constitute candidate adjacency pairs. For each pair of nodes... and Check elevation difference Does it exceed the maximum permissible terrain undulation? And the angle between the normal vectors of the local terrain planes of the two nodes. Does it exceed the terrain continuity threshold? If the above conditions are met, terrain continuity verification of the connecting section is also required: This is done by sampling along the straight line of the connecting section at intervals. Set sampling points, for each sampling point Query its nearest valid node and read the corresponding If all sampling points If all adjacent edges do not exceed the terrain traversability threshold, a bidirectional search for adjacent edges is established. To quantify whether a connecting segment traverses an impassable region, a connecting segment cost flag can be defined. As shown in formula ⑥:

[0159]

[0160] in:

[0161] : Connectivity flag, Boolean (value is 0 or 1). 1 indicates that a bidirectional search for adjacent edges can be established if all conditions are met, 0 indicates that it cannot be established.

[0162] The total number of verification sampling points set on the connection segment, a dimensionless positive integer. Calculated based on the connection segment length and sampling interval.

[0163] : Multiplication symbol, for products from arrive The indicator function values ​​at each sampling point are multiplied.

[0164] : Indicator function (also known as indicator function), takes the value 1 when the condition in parentheses is true, and takes the value 0 otherwise.

[0165] Sampling points The cost of passage at a location is dimensionless. This can be determined by querying the distance. The nearest valid node and read that node. (Obtained from formula ⑤)

[0166] Terrain passability threshold, dimensionless. Derived from robot access parameters, used to determine whether a node is passable.

[0167] Two valid nodes to be connected and Z-axis coordinate (elevation), dimensionless length (m).

[0168] Same as formula ⑤, the maximum allowable terrain undulation for the robot.

[0169] The angle between the local terrain plane normal vectors of two valid nodes, measured in radians. The calculation formula is as follows: ,in This is obtained from formula ②.

[0170] : Terrain continuity threshold, in radians (e.g., 30° corresponds to π / 6). From robot travel parameters.

[0171] : Absolute value operator (used for the absolute value of elevation difference and dot product of normal vectors).

[0172] Data source: The 3D coordinates and local terrain plane normal vectors of the "valid nodes" are derived from the calculations mentioned above in this step; "sampling points" "The 'travel cost' is obtained by linear interpolation of the connecting segments; it is given by formula ⑤; the 'robot travel parameters' are..." and search step size .

[0173] Simple numerical example: Suppose the elevations of two nodes are 1.2 meters and 1.35 meters respectively, with a difference of 0.15 meters. The distance is meters, satisfying the condition that the normal vectors are (0,0,1) and (0.05,0,0.998) respectively, the dot product is approximately 0.998, and the included angle is approximately 3.6°. The conditions are met; the connection section length is 2 meters, and the sampling interval is 0.5 meters. Each sampling point has a query result. The values ​​are 0.41, 0.42, 0.40, 0.43, and 0.41, respectively, all less than the threshold of 0.8. Establish adjacent edges. If the start or end node is marked as impassable in this step, re-query its neighborhood. Valid nodes not exceeding a threshold are designated as new endpoints; if none exist, an inaccessible endpoint is identified. The resulting 3D search graph includes a list of valid nodes, the 3D coordinates of each node, and the cost of travel. The search graph, along with the set of adjacent edges, is sent to the bidirectional search module of the S300. Terrain components, as part of the cumulative cost, are directly reused.

[0174] Technical effect of this section: By extracting and weighting the multi-dimensional terrain features of the local point cloud neighborhood, the terrain geometric attributes of the original point cloud are quantified into node-level accessibility cost. This cost is used to simultaneously complete node filtering and search graph adjacency constraints, realizing a compact mapping from perceived information to the planning graph structure. This avoids the information loss caused by binarized accessibility judgment and provides a continuously differentiable cost field for subsequent bidirectional search.

[0175] S300. Based on the 3D search map and obstacle point set, construct the obstacle distance distribution, expand the forward search end and reverse search end from the path start point and path end point, and include the passage cost and obstacle distance cost in the cumulative cost to generate candidate connection pairs:

[0176] The bidirectional search module receives the 3D search graph, starting node, ending node, and passage cost generated by S200, and simultaneously reads the obstacle point set generated by S100. Before path search begins, an obstacle distance distribution is constructed based on the obstacle point set. This obstacle distance distribution is used to query the distance from each valid node in the 3D search graph to the nearest obstacle point, and its data source is consistent with the obstacle distances used in subsequent connection path verification and smoothing path verification.

[0177] In one implementation, an independent Kd-Tree is established based on the obstacle point set. A nearest neighbor query is performed on each valid node to obtain the 3D Euclidean distance from that node to the nearest obstacle point. In another implementation, the obstacle point set is mapped to voxel space. The distance from each voxel to the nearest obstacle voxel is calculated through a 3D distance transformation, and the obstacle distance is read based on the voxel containing the valid node. Regardless of the method used, the distribution of obstacle distances retains the correspondence between valid node identifiers and obstacle distances.

[0178] For valid nodes whose obstacle distance is no greater than the robot's equivalent safe radius, the bidirectional search module deletes the node from the 3D search graph and simultaneously deletes the adjacent edges with that node as the endpoint. If the deleted node is a start node or an end node, a new node is selected from the neighboring valid nodes that is closer to the corresponding path endpoint and whose obstacle distance is greater than the robot's equivalent safe radius; if no node meeting the conditions is found, an insufficient start safe distance or insufficient end safe distance is indicated.

[0179] When the obstacle distance is greater than the robot's equivalent safety radius but less than the obstacle's expansion radius, a continuously decaying obstacle distance cost is generated based on the obstacle distance. This obstacle distance cost decreases as the obstacle distance increases, and decays to zero when the obstacle distance equals the obstacle's expansion radius. In one embodiment, the distance interval between the robot's equivalent safety radius and the obstacle's expansion radius is normalized, and then the obstacle distance cost is calculated using an exponential or quadratic function. When the obstacle distance is not less than the obstacle's expansion radius, the obstacle distance cost is set to zero.

[0180] After constructing the obstacle distance distribution, a forward open node set and a forward closed node set are established based on the starting node, and a reverse open node set and a reverse closed node set are established based on the ending node. The forward cumulative cost of the starting node is initialized to zero, and the reverse cumulative cost of the ending node is initialized to zero. Forward search takes the direction towards the ending node as the search direction, and reverse search takes the direction towards the starting node as the search direction. Each search direction maintains the node's cumulative cost, heuristic cost, composite evaluation value, and parent node relationship.

[0181] The cumulative cost of a candidate node is calculated recursively based on the cumulative cost of its parent node. The three-dimensional Euclidean distance between adjacent valid nodes is used as the geometric movement cost; the passage cost formed by the candidate node in S200 is used as the terrain cost; the cost of the candidate node in the obstacle distance distribution is used as the obstacle distance cost; the angle between the direction from the parent node to the current node and the direction from the current node to the candidate node is used to calculate the turning penalty. The geometric movement cost, passage cost, obstacle distance cost, and turning penalty are multiplied by their respective weights, added together, and accumulated to the cumulative cost of the parent node to form the cumulative cost of the candidate node.

[0182] When the current node is the first extended node in the search direction and there is no complete parent node direction, the turning penalty is set to zero. For the remaining nodes, adjacent direction vectors are constructed based on the 3D coordinates of the parent node, the current node, and the candidate nodes, and the angle between the two direction vectors is calculated; the larger the turning angle, the larger the turning penalty. The direction calculation in the reverse search is performed according to the search direction from the end node to the start node, and the path node arrangement direction is uniformly adjusted when the final path is stitched together.

[0183] The heuristic cost is calculated based on the 3D distance from the node to be expanded to the opposing target node. In the forward search, the opposing target node is the endpoint, and in the reverse search, it is the starting point. In one implementation, a power function mapping is performed on the 3D distance from the node to be expanded to the opposing target node to form the heuristic cost; the power exponent is given by the heuristic cost parameter, and when the power exponent equals one, the heuristic cost is a linear heuristic cost proportional to the 3D Euclidean distance. The power exponent remains constant throughout the same path planning task and is not represented as a dynamically changing weight during the search process.

[0184] The cumulative cost and heuristic cost of candidate nodes are added together to form a composite evaluation value. The bidirectional search module obtains the node with the smaller composite evaluation value from the forward open node set and the reverse open node set, respectively, and compares the current minimum composite evaluation value in both directions. In one implementation, the side with the smaller current minimum composite evaluation value is expanded first, so that the forward search end and the reverse search end advance alternately according to their respective search states. The node to be expanded is moved into the corresponding closed node set, and its search adjacent nodes are traversed.

[0185] When a searched neighboring node has not yet entered the corresponding closed node set, and the obstacle distance and node-level terrain accessibility constraints are met, a new cumulative cost to reach the searched neighboring node via the current node is calculated. If the new cumulative cost is less than the existing cumulative cost of the searched neighboring node, its cumulative cost and parent node are updated; if the searched neighboring node has not yet entered the open node set, it is added to the corresponding open node set. Nodes already in the closed node set are not repeatedly expanded, unless the current implementation is configured with a cost update rule that allows reopening nodes.

[0186] Candidate connection pairs between the forward and reverse search ends are formed based on spatial distance. When the spatial distance between a newly expanded node in the forward search and a node in the set of closed nodes in the reverse search is no greater than the connection distance threshold, or when the spatial distance between a newly expanded node in the reverse search and a node in the set of closed nodes in the forward search is no greater than the connection distance threshold, the corresponding two nodes are grouped into a candidate connection pair. This candidate connection pair is not used as the final path concatenation position but is instead sent to the S400 along with the node identifiers on both sides, the cumulative cost on both sides, and the parent node relationship for connection path verification.

[0187] When multiple candidate connection pairs exist, a ranking value is formed based on the forward cumulative cost, the reverse cumulative cost, and the connection distance between the two candidate nodes. Connection paths are then submitted for verification in ascending order of ranking value. If a candidate connection pair fails the S400 verification, the corresponding candidate is marked as an invalid candidate connection pair, and the bidirectional search module continues to expand either the forward or reverse search end. If both the forward and reverse open node sets are empty and no valid candidate connection pairs are found, a "no feasible path" identifier is output.

[0188] At the end of S300, the output includes the forward parent node relationship, the reverse parent node relationship, the forward search state, the reverse search state, the obstacle distance distribution, and candidate connection pairs. The passage cost, obstacle distance distribution, and parent node relationship do not terminate at this step but continue to participate in the connection path verification, initial path generation, and smooth path verification in S400.

[0189] S400: Verify the connection path based on the node-level terrain accessibility, minimum obstacle distance, and turning change of the candidate connection segment; if the verification passes, track the forward and reverse parent nodes to generate an initial path point sequence; generate a smooth path based on the initial path point sequence and verify it to obtain curve segments that do not meet the constraints; backtrack the path nodes or insert path nodes based on the curve segments that do not meet the constraints, regenerate and output the three-dimensional global path:

[0190] The path generation module receives the candidate connection pairs, forward parent node relationships, reverse parent node relationships, and obstacle distance distribution formed in S300, and reads the passage cost formed in S200. For candidate connection pairs with higher current sorting positions, the straight line segment between the forward search node and the reverse search node is taken as the connection segment to be verified, and multiple verification sampling points are set according to the connection path verification sampling interval. The connection path verification sampling interval is not greater than the search step size, and the two endpoints of the connection segment are also used as verification sampling points for inspection.

[0191] The node-level terrain accessibility of each verification sampling point is obtained through neighboring valid nodes. In one implementation, the nearest valid node to the verification sampling point is queried, and the access cost of that node is read; when the distance between the verification sampling point and the nearest valid node exceeds the node mapping distance threshold, the verification sampling point is identified as being outside the valid terrain region. In another implementation, multiple valid nodes around the verification sampling point are queried, and an inverse distance weighting is performed based on the distance between each valid node and the verification sampling point to obtain the node-level terrain accessibility corresponding to the verification sampling point.

[0192] If any verification sampling point cannot be mapped to a valid node, or if its node-level terrain accessibility exceeds the terrain accessibility threshold, the terrain verification of the connection segment fails. For successfully mapped verification sampling points, their obstacle distances are read from the obstacle distance distribution; when constructing the obstacle distance distribution using the point cloud nearest neighbor method, the 3D Euclidean distance from the verification sampling point to the nearest obstacle point is directly queried. The minimum obstacle distance among all verification sampling points is taken as the minimum obstacle distance of the connection segment. If the minimum obstacle distance is less than the robot's equivalent safety radius, the obstacle verification of the connection segment fails.

[0193] The steering change is calculated separately at both ends of the connection segment. On the forward search side, the first steering angle is calculated based on the direction from the parent node of the forward candidate node to the forward candidate node and the direction of the connection segment from the forward candidate node to the reverse candidate node. On the reverse search side, the reverse parent node chain is first adjusted so that the direction points from the candidate connection pair to the end point of the path, and then the second steering angle is calculated based on the connection segment direction and the direction from the reverse candidate node to its successor node. If either the first or second steering angle exceeds the steering angle threshold, the connection segment direction verification fails.

[0194] When the node-level terrain accessibility, minimum obstacle distance, and turning change of a connection segment all meet the corresponding constraints, a connection path verification result is generated. If the current candidate connection pair fails any of the verifications, the path generation module marks the candidate connection pair as an invalid connection segment and notifies the bidirectional search module to continue expanding the forward or reverse search end; node combinations already marked as invalid connection segments will not be resubmitted. This process avoids directly merging paths simply because two search fronts are spatially close.

[0195] After the connection segment passes verification, the path generation module traces backward from the forward candidate node along the forward parent node relationship to the starting node, and reverses the tracing results to form a forward path node sequence from the starting node to the forward candidate node; it also traces backward from the reverse candidate node along the reverse parent node relationship to the ending node to form a reverse path node sequence from the reverse candidate node to the ending node. The forward path node sequence, the connection segments corresponding to the candidate connection pairs, and the reverse path node sequence are concatenated, and nodes with duplicate connection positions are deleted to form the initial path point sequence.

[0196] The initial path point sequence enters the path simplification process. Starting from the initial path origin, the path generation module selects subsequent non-contiguous nodes according to the node arrangement order, and uses the straight line segment between the currently retained node and the non-contiguous node as the cross-node connection segment. The cross-node connection segment sets sampling points in the same way as the connection path verification, and queries the node-level terrain accessibility and obstacle distance. When all sampling points of the cross-node connection segment meet the terrain accessibility threshold and the robot's equivalent safety radius constraint, the intermediate node between the two endpoints is deleted; if the verification fails, the path node that caused the cross-node connection to fail is retained, and subsequent simplification continues from that path node.

[0197] During path simplification, subsequent nodes that are far from the currently retained nodes and have passed verification are prioritized. Intermediate nodes consecutively located within the same passable corridor are deleted, while necessary nodes near obstacle corners, slope transitions, and terrain edges are retained. After simplification, a simplified path node sequence is formed. A node index correspondence is maintained between this simplified path node sequence and the initial path point sequence. When a curve segment that does not meet the constraints is subsequently detected, the deleted initial path point is located based on this index correspondence.

[0198] This implementation uses a simplified path node sequence as path nodes to construct a cubic B-spline curve. The cubic B-spline employs an open node vector, ensuring that the curve's start and end points correspond to the path's start and end points, respectively. When the number of simplified path nodes meets the control point requirement for the cubic B-spline, path nodes are directly established according to the node arrangement order. When the number of control points is insufficient, in one implementation, the first and last control points are repeatedly configured to ensure the number of control points meets the curve calculation requirements. In another implementation, for paths with insufficient nodes, segmented straight paths are retained, and curve replacement is not performed.

[0199] After the smooth path is formed, the path verification module sets multiple path sampling points on the curve according to the smooth path sampling interval. For each path sampling point, the corresponding node-level terrain accessibility and obstacle distance are queried, and the query method is the same as that for sampling points in the connection path verification. If a path sampling point cannot be mapped to a valid node, the corresponding node-level terrain accessibility exceeds the terrain accessibility threshold, or the obstacle distance is less than the robot's equivalent safe radius, the path sampling point is marked as an unqualified path sampling point.

[0200] Following the parameter arrangement order on the smooth path, consecutively occurring unqualified path sampling points are merged into curve segments that do not meet the constraints. For each unqualified curve segment, its starting and ending parameter positions within the smooth path are recorded, and its corresponding simplified path node and initial path point are found based on the influence range of the B-spline control points. The path verification module does not re-search the entire path; instead, it adjusts the path nodes for the unqualified curve segments.

[0201] In one implementation, initial path points located within the range of curve segments that do not meet the constraints and were deleted during the path simplification stage are recovered from the initial path point sequence and inserted into the simplified path node sequence according to their original arrangement. The recovered initial path points are derived from paths that have already passed search and cross-node connection verification, and their three-dimensional coordinates are consistent with the original initial paths. After insertion, a cubic B-spline curve is reconstructed, and the path sampling points for the affected curve segments and their adjacent segments are reset.

[0202] In another implementation, if there are no deleted initial path points within the range corresponding to the curve segment that does not meet the constraints, or if the verification still fails after backtracking the path node, a path node is inserted into the curve segment that does not meet the constraints. The insertion position is selected near the path sampling point with a large travel cost or a small obstacle distance in the curve segment that does not meet the constraints. Then, from its neighboring valid nodes, nodes with a travel cost not exceeding the terrain passability threshold and an obstacle distance not less than the robot's equivalent safety radius are selected, and the three-dimensional coordinates of the node are used as the path node. When there are multiple candidate nodes, they are sorted according to the distance from the candidate node to the original curve and the travel cost, and the node with the smaller sort value is selected.

[0203] Each time a path node is rolled back or inserted, the path correction count increases by one, and a smooth path is regenerated. The regenerated smooth path continues to undergo node-level terrain accessibility and obstacle distance checks until no curve segments fail to meet the constraints, or the path correction count reaches the correction count threshold. When the curve check passes, the smooth path is determined as the 3D global path.

[0204] If the number of path corrections reaches the correction threshold and the smoothed path still contains curve segments that do not meet the constraints, no more path nodes are added. At this point, it is checked whether the initial path point sequence maintains a valid connection path verification state. If the initial path point sequence is valid, it is output as the 3D global path with an initial path rollback flag. If the initial path point sequence fails, the node range corresponding to the currently failed segment is returned to the bidirectional search module. This node range is marked as a prohibited connection area in subsequent searches, and candidate connection pairs are regenerated.

[0205] The final output 3D global path is arranged in order from the path start point to the path end point, and contains the 3D coordinates of multiple path points. The path direction is calculated based on the direction vectors between adjacent path points, which is used by the robot navigation controller for path tracking. Before output, the mapping relationship between the path start point and the path end point is checked again; when the endpoint mapping distance exceeds the endpoint mapping radius, the connection segment between the start point node or end point node and the actual path endpoint that has been verified by terrain and obstacles is retained.

[0206] The 3D global path is sent to the robot navigation controller by the path verification module. The robot navigation controller performs subsequent trajectory generation or path tracking according to the path point sequence. In this embodiment, the global path planning process ends after the path is output. When the map changes or the robot receives a new path endpoint, the point cloud preprocessing module rereads the corresponding 3D point cloud map and path endpoints, and starts a new round of processing from S100 to S400.

[0207] In one specific embodiment:

[0208] In the S400 path generation module, it first receives candidate connection pairs, forward parent node relationships, reverse parent node relationships, and obstacle distance distribution from S300, and reads the passage cost generated by S200. For the candidate connection pairs that are currently ranked higher, it performs a forward search on the nodes. With reverse search node The straight line segments between them are used as the connection segments to be verified, and the sampling interval is verified according to the connection path. (Not greater than the search step size) setting Each sampling point is a verification sampling point, including both endpoints. The nearest valid node is queried using the point cloud spatial index, and its passage cost is read. Simultaneously, read obstacle distances from the obstacle distance distribution. To quantify the overall security of the connection segment, a connection path verification pass flag is defined. It must simultaneously satisfy terrain, obstacle, and steering constraints. The minimum obstacle distance for the connecting section is... Calculate according to formula ⑦:

[0209]

[0210] in:

[0211] : Minimum obstacle distance to all verification sampling points on the connection segment, expressed in length (meters). Obtained by querying the obstacle distance distribution.

[0212] : Minimum function, for from arrive The obstacle distance at each sampling point is taken as the minimum value.

[0213] : Sampling point index, a dimensionless positive integer.

[0214] : The total number of verification sampling points set on the connection segment, a dimensionless positive integer. Calculated by dividing the connection segment length by the sampling interval. The result is obtained by rounding down.

[0215] : No. sampling points The distance to obstacles at a given location is expressed in meters (length). This field is obtained from an obstacle distance distribution query and is constructed by the S300 based on a set of obstacle points.

[0216] : No. The three-dimensional coordinate vector of each verification sampling point is expressed in units of length (meters). It is obtained by linear interpolation of the line segments between the forward search node and the reverse search node.

[0217] Data source: "Verify sampling point coordinates" "Obstacle distance distribution" is obtained by linear interpolation of the connection segment; "obstacle distance distribution" comes from S300, which stores the Euclidean distance from each valid node and any point in space to the nearest obstacle point.

[0218] Simple numerical example: Assume the length of the connecting section is 2 meters. Rice, then At each sampling point, the measured distances to the obstacle were 0.25 meters, 0.30 meters, 0.28 meters, 0.22 meters, and 0.26 meters, respectively. meters. If the robot's equivalent safety radius Rice, then The obstacle check failed. The steering change is calculated at both ends of the connection segment: on the forward search side, based on the direction vector from the parent node of the forward candidate node to the forward candidate node. With the direction vector of the connecting segment Calculate the first steering angle On the reverse search side, first adjust the reverse parent node chain so that it points from the candidate connection pair to the end of the path, then adjust it according to the direction of the connection segment. and the direction vector from the reverse candidate node to its successor node Calculate the second steering angle The steering angle calculation function is defined as shown in formula ⑧:

[0219]

[0220] in:

[0221] Steering angle, measured in radians, with values... . represents the absolute value of the angle between two direction vectors.

[0222] : Inverse cosine function, returns the value in radians.

[0223] : Absolute value sign (here, the absolute value of the dot product result is taken to ensure that the angle is acute).

[0224] The first direction vector, with dimensions of length (meters) or dimensionless (if normalized). On the forward search side, it is the vector from the parent node to the forward candidate node; on the reverse search side, it is the direction vector of the connecting segment.

[0225] The second direction vector has the dimension of length (meters) or is dimensionless. On the forward search side, it is the direction vector of the connecting segment; on the reverse search side, it is the vector from the reverse candidate node to its successor node.

[0226] : Vector Euclidean norm operator.

[0227] Data source: "Direction vector" "On the forward side, take the vector from the parent node to the forward candidate node; on the reverse side, take the direction vector of the connection segment." (Direction vector) "On the positive side, take the direction vector of the connecting segment, and on the negative side, take the vector from the negative candidate node to its successor node."

[0228] Simple numerical example: Let , The dot product is 0.866, and the modulus is 1. Radius ≈ 30°. If the steering angle threshold... (0.7854 radians), then 30° passes the check. When And both steering angles do not exceed And all sampling points If all connections can be mapped to valid nodes, the connection path verification passes; otherwise, the connection segment is marked as invalid, and S300 is notified to continue expansion. This section outputs the connection path verification result. Minimum obstacle distance and two steering angles It is directly used in the initial path generation step of S400.

[0229] After the connection path verification passes, the path generation module traces backward from the forward candidate nodes along the forward parent node relationship to the starting node, and arranges them in reverse order to obtain the forward path node sequence; it then traces backward from the reverse candidate nodes along the reverse parent node relationship to the ending node to obtain the reverse path node sequence; finally, the forward sequence, connection segment, and reverse sequence are concatenated and duplicate nodes are removed to form the initial path point sequence. ,in This represents the total number of initial path points. The initial path sequence undergoes simplified path processing: starting from the starting point... Begin by selecting subsequent non-contiguous nodes according to the node arrangement order. ( ), will retain the current node and The straight line segments between nodes are used as cross-node connection segments. Sampling points are set in the same way as for connection path verification, and the passage cost and obstacle distance are queried. To quantify whether a cross-node connection segment is passable, a cross-node connection segment cost is defined. This cost is determined by the proportion of sampling points that do not meet the constraints, as shown in formula ⑨:

[0230]

[0231] in:

[0232] : The proportion of impassable segments across nodes, dimensionless, with values ​​ranging from... This indicates the proportion of sampling points that do not meet the constraints of terrain or obstacles.

[0233] The total number of sampling points on the cross-node connection segment, a dimensionless positive integer. It is obtained by dividing the length of the cross-node connection segment by the sampling interval.

[0234] : Summation symbol, for summation from arrive The indicator function values ​​for each sampling point are summed.

[0235] : Sampling point index, a dimensionless positive integer.

[0236] Indicator function. It takes the value 1 if the condition within the parentheses is true, and 0 otherwise.

[0237] : No. The travel cost per sampling point is dimensionless. This is determined by querying the distance. The nearest valid node and read it. The data source is S200.

[0238] Terrain passability threshold, dimensionless. Derived from robot access parameters.

[0239] The logical "OR" operator means that if one of the two conditions is true, the indicator function will be set to 1.

[0240] : No. The obstacle distances at each sampling point are measured in meters (length). The distances are obtained from an obstacle distance distribution query, with the data source being S300.

[0241] The robot's equivalent safety radius, measured in length (meters). Derived from robot travel parameters, it is used to determine whether the distance to obstacles is safe.

[0242] : No. The three-dimensional coordinate vectors of the sampling points of the cross-node connection segment are expressed in units of length (meters). They are obtained by linear interpolation of the line segments between the currently retained node and the discontinuous node.

[0243] Data source: "sampling points" "Obtained by linear interpolation of cross-node connection segments; "Passage cost" comes from S200; "Obstacle distance distribution" comes from S300.

[0244] Simple numerical example: Suppose there is a cross-node connection segment with There are 2 sampling points. 1 sampling point (When counting overlap, the indicator function is used to judge independently. If the same sampling point violates both conditions at the same time, it is only counted as 1.) Therefore, the number of sampling points that do not meet the constraints is 3. .when When all sampling points satisfy the constraints, all intermediate nodes between the two endpoints are deleted; when The path node that caused the cross-node connection to fail is retained, and subsequent simplification continues from that node. During the simplification process, nodes that are farther away from the currently retained node are prioritized. The subsequent nodes are then removed, thus eliminating intermediate nodes consecutively located within the same passable corridor, while retaining necessary nodes near obstacle corners, slope transitions, and terrain edges. This simplification results in a simplified path node sequence. ( This section outputs a simplified path node sequence, preserving the node index correspondence between the simplified path node sequence and the initial path node sequence. And the index mapping table, which is used by the smooth path generation step of S400.

[0245] Following the previously mentioned simplified path node sequence, the path generation module uses these as path nodes to construct a cubic B-spline curve. An open node vector is employed, ensuring that the curve's start and end points correspond to the path's start and end points, respectively. Let the number of control points be... The parametric equation of the cubic B-spline curve is: ,in The basis functions are cubic B-spline functions. After the smooth path is formed, the path verification module performs the smoothing sampling according to the path smoothing interval. In the parameter domain Multiple path sampling points are set up, each mapped to 3D coordinates, and the passage cost and obstacle distance are queried. When multiple consecutive sampling points do not meet the constraints, they are merged into a curve segment that does not meet the constraints. For each curve segment that does not meet the constraints, a local correction strategy is adopted: priority is given to correcting the initial path point sequence. The process involves restoring the initial path points located within the corresponding range of the unqualified segment that were deleted during the simplification phase. If the restored path points still do not meet the requirements, new path nodes are inserted. To select the optimal insertion control point from the valid nodes near the unqualified segment, a candidate node scoring function is defined as shown in formula 10:

[0246]

[0247] in:

[0248] : Dimensionless score of candidate nodes. The smaller the value, the more suitable the node is as a path node for insertion.

[0249] First weighting coefficient, dimensionless, and satisfy And it is non-negative. It is used to balance the importance of the distance and cost terms.

[0250] Second weighting coefficient, dimensionless, same as above.

[0251] Euclidean norm: used to calculate the length of a vector.

[0252] : The three-dimensional coordinate vector of the candidate valid node, with the dimension of length (meters). It is derived from the set of valid nodes selected by S200.

[0253] : The three-dimensional coordinate vector of the curve sampling point closest to the candidate node in the curve segment that does not meet the constraints, with the dimension of length (meters). It is calculated from the parametric equation of the current smoothing path.

[0254] Reference distance, measured in length (meters). This can be taken as the search step size or robot footprint size, used for dimensionless distance terms.

[0255] Candidate nodes The passage cost is dimensionless. It is obtained from S200 and the cost of the node is read directly.

[0256] Data source: "Candidate valid nodes" comes from the set of valid nodes filtered by S200; "Curve sampling points" "Calculated from the parametric equations of the current smoothing path"; "Weight coefficients" "Based on the preset correction bias, for example, if the bias is closer to the original curve, then take..." .

[0257] Simple numerical example: Let Rice, candidate node curve sampling points ,distance rice, ,Pick ,but If another candidate node scores 0.62, the node with the lower score is selected as the insertion control point. Each time a path node is rolled back or inserted, the path correction count is incremented by 1, the B-spline curve is regenerated and validated again, until no curve segments fail to meet the constraints or the correction count threshold is reached. If unqualified segments still exist after reaching the threshold, the validity of the initial path sequence is checked: if the initial path sequence itself passes validation, it is output as the 3D global path with an initial path rollback flag attached; otherwise, the node range corresponding to the failed segment is returned to S300, marked as a prohibited connection area, and candidate connection pairs are regenerated. The final output 3D global path, arranged in order from start to finish, contains the 3D coordinates of multiple path points and is used by the robot navigation controller for path tracking. This section outputs the 3D global path coordinate sequence and path direction vector, which are directly received by the robot navigation controller.

[0258] The technical effects of this section are as follows: Multiple checks on the connection segments ensure splicing safety; redundant nodes are simplified and deleted by using nodes based on terrain and obstacle constraints; and a local correction strategy of prioritizing backtracking path nodes and then inserting new control points is adopted. This achieves closed-loop iterative optimization from discrete search nodes to executable smooth curves, avoiding the high computational cost of global replanning, while ensuring the geometric continuity and physical feasibility of the path in complex 3D terrain.

[0259] Example 2: Figure 2 A structural block diagram of a bidirectional search path planning system based on three-dimensional point cloud terrain assessment according to an embodiment of the present invention is shown. Figure 2 As shown, the structure may include:

[0260] The point cloud preprocessing module 01 is used to acquire a 3D point cloud map, a path start point and a path end point, separate terrain points and obstacle points in the 3D point cloud map, and generate a candidate node set and an obstacle point set based on the terrain points.

[0261] The point cloud preprocessing module is connected to the 3D point cloud map storage interface and the path task input interface. Upon receiving a path planning task, it reads the 3D point cloud map, path start point, and path end point, all within the same world coordinate system. The 3D point cloud map comes from a pre-built static environment map of the ground mobile robot or from point cloud files output by external mapping equipment. The path start point corresponds to the robot's current location, and the path end point corresponds to the location specified in this navigation task. When the coordinate systems used by the three are inconsistent, the point cloud preprocessing module unifies the coordinates according to a pre-configured coordinate transformation relationship; if no valid coordinate transformation relationship is found, the current task is paused, a coordinate system anomaly marker is generated, and no candidate node set is output to subsequent modules.

[0262] After reading the 3D point cloud map, the point cloud preprocessing module extracts the point cloud region to be planned based on the path start point, path end point, and the spatial range between them. Then, it performs voxel downsampling and statistical outlier filtering on this region. Voxel downsampling replaces the original point cloud with the centroid of the point cloud within each voxel, while statistical outlier filtering removes isolated points based on the distance distribution between the current point and its neighbors. The processed point cloud retains ground slope variations, step edges, and obstacle outlines, and establishes a spatial index for neighborhood queries.

[0263] The separation of terrain points from obstacle points is achieved based on point cloud normal vectors and local elevation differences. The point cloud preprocessing module queries the local neighborhood for each preprocessed point and calculates the local normal vector based on the coordinates of the neighboring points. If the angle between the normal vector and the vertical direction of the world coordinate system is within the allowable range of the terrain surface, and the elevation difference of the current point relative to the neighboring reference surface does not exceed the terrain point separation threshold, the current point is added to the terrain point set. Points with normal vectors close to the horizontal direction, abrupt changes in local elevation, or located above the terrain surface are added to the obstacle point set. When the number of points in the local neighborhood is insufficient, the corresponding point is not directly added to the terrain point set but is retained as a point with insufficient point cloud coverage for exclusion during subsequent terrain accessibility assessments.

[0264] After the terrain point set is formed, the point cloud preprocessing module spatially samples the terrain points according to the search step size, selecting terrain points with valid elevations as candidate terrain nodes within the sampling area of ​​each node. When multiple elevation layers exist at the same horizontal position, the candidate terrain nodes corresponding to each elevation layer are retained separately to prevent the merging of upper and lower platforms or stair structures in the horizontal projection. The path start and path end points are mapped to adjacent candidate terrain nodes. If the mapping distance exceeds the endpoint mapping range, a new query is performed within the expanded adjacent area. If no candidate terrain node matching the terrain point criteria is found, an invalid path endpoint is marked and the current processing branch ends.

[0265] The point cloud preprocessing module outputs a candidate node set, an obstacle point set, and candidate terrain nodes corresponding to the path start and end points. The candidate node set is transmitted to the terrain assessment and search map construction module, while the obstacle point set is simultaneously transmitted to the bidirectional search module. Both candidate terrain nodes and obstacle points retain three-dimensional coordinates, enabling subsequent local point cloud neighborhood queries, obstacle distance calculations, and path sampling point mapping to use the same spatial reference.

[0266] The terrain assessment and search map construction module 02 is connected to the point cloud preprocessing module. It is used to establish a local point cloud neighborhood based on the candidate node set and fit a local terrain plane to generate local terrain features. It determines the passage cost based on the local terrain features, uses the passage cost to filter effective nodes and establish adjacency relationships, and generates a three-dimensional search map.

[0267] After receiving the candidate node set, the terrain assessment and search map construction module reads the three-dimensional coordinates of each candidate terrain node according to its identifier and calls the spatial index formed by the point cloud preprocessing module to query the corresponding local point cloud neighborhood. The spatial scale of the local point cloud neighborhood is configured according to the footprint range and search step size of the ground mobile robot, so that the neighborhood range covers the terrain bearing area corresponding to the robot when it is located at the current candidate terrain node. If the number of neighborhood points is lower than the neighborhood point number threshold, or the point cloud coverage of the neighborhood in multiple directions is lower than the point cloud coverage threshold, the corresponding candidate terrain node is marked as an impassable point and is not included in the local terrain plane fitting.

[0268] For candidate terrain nodes with effective local point cloud neighborhoods, the terrain assessment and search graph construction module fits a local terrain plane based on the centroid and covariance relationship of the neighborhood points, and obtains the normal vector of the local terrain plane. When the local point cloud neighborhood contains different slopes or terrain edges, the dispersion of the neighborhood points to the local terrain plane increases; when the plane fitting result undergoes directional degradation and cannot form a stable normal vector, the corresponding candidate terrain node is marked as an impassable point and does not participate in the construction of the 3D search graph.

[0269] Local terrain features include slope, flatness, point cloud sparsity, and edge features. Slope is calculated based on the angle between the normal vector of the local terrain plane and the vertical direction of the world coordinate system; flatness is calculated based on the distance distribution from local neighboring points to the local terrain plane; point cloud sparsity is calculated based on the number of directions without point cloud coverage and the void ratio within the local point cloud neighborhood; edge features are extracted based on the elevation difference within the local point cloud neighborhood. To avoid a single residual outlier directly determining edge features, the elevation difference can be calculated by selecting high and low points according to the elevation sorting results of the local point cloud.

[0270] The terrain assessment and search map construction module normalizes the terrain features according to the reference range corresponding to each feature, and then performs weighted fusion based on pre-configured terrain feature weights to obtain the passage cost corresponding to each candidate terrain node. When the passage cost exceeds the terrain passability threshold, the corresponding candidate terrain node is marked as an impassable point; the remaining candidate terrain nodes are determined as valid nodes. The passage cost maintains its correspondence with the valid nodes after node screening, and is subsequently included in the cumulative cost by the bidirectional search module.

[0271] The adjacency relationships between valid nodes are established based on 3D Euclidean distance and terrain continuity. The terrain assessment and search graph construction module queries valid nodes within the search step size, compares the elevation difference and local terrain plane orientation between two nodes, and sets multiple sampling positions along the connection segment between the two nodes. If any sampling position cannot be mapped to a valid node, or if the passage cost of the mapped node exceeds the terrain traversability threshold, no corresponding adjacency relationship is established. Search adjacency edges are established between valid nodes that satisfy the distance constraint and terrain continuity constraint, forming a 3D search graph containing the node's 3D coordinates, passage cost, and adjacency relationships.

[0272] When candidate terrain nodes corresponding to the path start or end point are eliminated, the terrain assessment and search map construction module queries the vicinity of the corresponding path endpoint for valid nodes whose travel cost does not exceed the terrain traversability threshold, and remaps the path endpoints. If no valid nodes exist in the vicinity, a path endpoint is marked as traversable, and the bidirectional search module does not initiate the current path search. The 3D search map, travel cost, and remapped path endpoints are sent together to the bidirectional search module.

[0273] The bidirectional search module 03 is connected to the point cloud preprocessing module and the terrain assessment and search map construction module, respectively. It is used to construct the obstacle distance distribution based on the three-dimensional search map and the obstacle point set, expand the forward search end and the reverse search end from the path start point and the path end point, include the passage cost and obstacle distance cost in the cumulative cost, and generate candidate connection pairs, forward parent node relationships and reverse parent node relationships.

[0274] The bidirectional search module receives a 3D search graph, passage cost, obstacle point set, and valid nodes corresponding to the path start and end points. Before the path search begins, a spatial query structure is established based on the obstacle point set, and the obstacle distance from each valid node to the nearest obstacle point is calculated, forming an obstacle distance distribution. When the obstacle distance of a valid node is not greater than the robot's equivalent safety radius, the corresponding valid node and its search adjacent edges are deleted from the 3D search graph; when the obstacle distance is between the robot's equivalent safety radius and the obstacle expansion radius, a continuously decreasing obstacle distance cost is configured according to the distance size; when the obstacle distance is not less than the obstacle expansion radius, the corresponding obstacle distance cost is set to zero.

[0275] When deleting a node involves the start or end point of a path, the bidirectional search module re-queries for nearby valid nodes that satisfy the robot's equivalent safety radius constraint in the 3D search graph. If no alternative node is found, the current path search stops, and a marker indicating insufficient safety distance to the path endpoint is generated. After successful remapping, the forward search end is initialized with the valid node corresponding to the path start point, and the reverse search end is initialized with the valid node corresponding to the path end point; the two search directions respectively maintain open nodes, closed nodes, cumulative costs, and parent node relationships.

[0276] The node expansion in both the forward and reverse search ends uses the same cost structure. When the current node expands towards adjacent valid nodes, the bidirectional search module reads the three-dimensional Euclidean distance between adjacent nodes, the passage cost corresponding to the candidate node, and the obstacle distance cost, and forms a turning penalty based on the turning angle between the parent node direction and the candidate node direction. These costs are written into the cumulative cost of the current search direction according to the configured weights. If the current node is the starting node of the search and lacks a complete parent node direction, the corresponding turning penalty is set to zero.

[0277] The three-dimensional distance from the candidate node to the endpoint of the opposing path is used to form the heuristic cost. Forward search takes the direction towards the path endpoint as the opposing search direction, while backward search takes the direction towards the path start point as the opposing search direction. In one implementation, the heuristic cost is obtained by performing a power function mapping on the three-dimensional distance, with the power exponent remaining constant throughout a single path planning task. The cumulative cost and the heuristic cost together form a composite evaluation value for the node, and the bidirectional search module selects nodes to be expanded from either the forward or backward search end based on this composite evaluation value.

[0278] After the node to be expanded is moved into the set of closed nodes in the corresponding search direction, the bidirectional search module traverses its search neighboring nodes. When the newly calculated cumulative cost is less than the existing cumulative cost of the search neighboring node, the corresponding cumulative cost and parent node are updated; search neighboring nodes that have not yet entered the set of open nodes are added to the corresponding search frontier. Forward search and reverse search are expanded alternately, and the alternation order is determined according to the magnitude of the current composite evaluation value of the two search directions.

[0279] When the spatial distance between a node in the forward search and a node already visited in the reverse search is no greater than the connection distance threshold, the bidirectional search module combines the corresponding nodes to form a candidate connection pair. When multiple candidate connection pairs appear simultaneously, they are sorted according to the forward cumulative cost, the reverse cumulative cost, and the distance between the two connected nodes. The candidate connection pairs with higher rankings are sent to the connection path verification and initial path generation module first. If a candidate connection pair fails the connection path verification, the bidirectional search module retains the original search state and continues to expand the forward or reverse search; the corresponding node combination is marked as an invalid candidate connection pair and is not submitted again.

[0280] During the search process, if both the forward open node set and the reverse open node set are empty and no candidate connection pairs have passed the verification, a "no feasible path" marker is generated. After candidate connection pairs are formed, the bidirectional search module synchronously outputs the forward parent node relationship, the reverse parent node relationship, and the obstacle distance distribution, enabling the connection path verification and initial path generation module to trace the node chains in both search directions and check the connection segments according to the same obstacle distance benchmark.

[0281] The connection path verification and initial path generation module 04 is connected to the terrain assessment and search map construction module and the bidirectional search module, respectively. It is used to verify the connection path based on the node-level terrain accessibility, minimum obstacle distance and turning change of the corresponding connection segment according to the candidate connection. When the connection path verification is passed, it generates an initial path point sequence based on the positive parent node relationship and the reverse parent node relationship.

[0282] The connection path verification and initial path generation module receives candidate connection pairs, forward parent node relationships, reverse parent node relationships, and obstacle distance distribution from the bidirectional search module, and reads the passage cost from the terrain assessment and search map construction module. Candidate connection pairs include forward search nodes and reverse search nodes; the straight line segment between two nodes is determined as the connection segment to be verified. The connection path verification and initial path generation module sets multiple verification sampling points on the connection segment to be verified at a sampling interval no greater than the search step size.

[0283] Each verification sampling point obtains its corresponding travel cost through its nearest valid node. If the distance from the verification sampling point to the nearest valid node exceeds the node mapping distance threshold, it indicates that the connection segment has left the coverage area of ​​the valid terrain node, and the terrain verification of the current candidate connection pair fails. If the mapping is successful, the travel cost of the nearest valid node is read; if the travel cost corresponding to any verification sampling point exceeds the terrain passability threshold, the current connection segment does not meet the terrain continuity constraint.

[0284] The obstacle distances for the connecting segments are read from the obstacle distance distribution or obtained through a nearest neighbor query between the verification sampling points and the obstacle point set. The minimum obstacle distance among multiple verification sampling points is taken as the minimum obstacle distance for the connecting segment. If the minimum obstacle distance is less than the robot's equivalent safety radius, the obstacle verification for the connecting segment fails.

[0285] The steering changes are calculated at both ends of the connection segment. The parent node of a forward connection node forms the forward path direction to the forward connection node, and the forward connection node forms the connection segment direction to the reverse connection node. The reverse connection node forms the reverse path direction after changing direction according to the final path direction. The connection path verification and initial path generation module calculates the steering angle between the forward path direction and the connection segment direction, and the steering angle between the connection segment direction and the reverse path direction, respectively. If any steering angle exceeds the steering angle threshold, the steering verification of the current candidate connection pair fails.

[0286] When the node-level terrain accessibility, minimum obstacle distance, and turning change all meet the corresponding constraints, a connection path verification result is generated. If the verification fails, the candidate connection pairs and the type of failure are fed back to the bidirectional search module, which then continues to expand the search frontier accordingly. The connection path verification and initial path generation module does not change the access cost and obstacle distance distribution in the 3D search graph; it only rejects the connection relationships corresponding to the current node combination.

[0287] After the connection path verification passes, the connection path verification and initial path generation module traces the path from the forward connection node along the forward parent node relationship to the path start point, and reverses the traced nodes to form a forward path node sequence; it also traces the path from the reverse connection node along the reverse parent node relationship to the path end point to form a reverse path node sequence. The forward path node sequence, the nodes at both ends of the connection segment, and the reverse path node sequence are concatenated in the direction from the path start point to the path end point, and duplicate nodes are deleted to form the initial path point sequence.

[0288] Each node in the initial path point sequence retains its node identifier and 3D coordinates in the 3D search graph. This facilitates the path smoothing and detection module in reading the passage cost based on the node identifier and in locating the corresponding original initial path point when local failures occur in path smoothing. The connection path verification and initial path generation module sends the initial path point sequence, obstacle distance distribution, and node index correspondence to the path smoothing and detection module.

[0289] The path smoothing and detection module 05 is connected to the connection path verification and initial path generation module. It is used to generate a smooth path based on the initial path point sequence, perform path feasibility detection on the smooth path according to the passage cost and the obstacle distance distribution, obtain curve segments that do not meet the constraints, backtrack the path nodes or insert path nodes according to the curve segments that do not meet the constraints, and regenerate and output the three-dimensional global path.

[0290] After receiving the initial path point sequence, the path smoothing and detection module first checks the cross-node connection segments between adjacent non-contiguous nodes according to the node arrangement order. Each cross-node connection segment has multiple path checkpoints, and the corresponding passage cost and obstacle distance are queried. If all path checkpoints are within the effective terrain coverage area, the passage cost does not exceed the terrain passability threshold, and the obstacle distance is not less than the robot's equivalent safe radius, the intermediate node between the two endpoints of the cross-node connection segment is deleted; if any path checkpoint fails the check, the corresponding intermediate node is retained. This process forms a simplified path node sequence.

[0291] The simplified path node sequence is used as the input path node in the cubic B-spline curve calculation process to generate a smooth path. When the number of simplified path nodes does not meet the control point requirements of the cubic B-spline, the straight line connection of the corresponding segment is retained, or the first and last path nodes are repeatedly configured. The path start and end points remain the first and last points of the smooth path, and the endpoints do not shift due to curve fitting.

[0292] After the smooth path is formed, the path smoothing and detection module sets multiple path sampling points according to the path sampling interval. Each path sampling point obtains its corresponding passage cost through neighboring valid nodes and its corresponding obstacle distance from the obstacle distance distribution. A path sampling point is marked as an unqualified path sampling point if it cannot be mapped to a valid node, its passage cost exceeds the terrain passability threshold, or its obstacle distance is less than the robot's equivalent safe radius.

[0293] Non-compliant path sampling points continuously distributed along the smooth path are merged into curve segments that do not meet the constraints. The path smoothing and detection module finds the corresponding simplified path nodes and initial path points based on the position of the non-constrained curve segments in the smooth path. If there are nodes in the initial path point sequence that are located in the corresponding segment and were deleted during the node simplification stage, the initial path points are restored to the simplified path node sequence, and the smooth path is regenerated.

[0294] If no recoverable initial path point is found, or if the corresponding curve segment still fails verification after reverting to a path node, the path smoothing and detection module inserts a path node into the curve segment that does not meet the constraints. Path nodes are selected from valid nodes near the curve segment that does not meet the constraints. The passage cost of candidate nodes must not exceed the terrain passability threshold, and the obstacle distance must not be less than the robot's equivalent safety radius. If multiple candidate nodes exist, they are sorted according to their distance to the original smoothed path and their passage cost, and the candidate node with the highest ranking is selected as the path node.

[0295] After each path node rollback or insertion, the path smoothing and detection module regenerates the smoothed path and resets the path sampling points to perform path feasibility testing. If the regenerated smoothed path does not contain any curve segments that do not meet the constraints, the smoothed path is output as a 3D global path. If the number of path corrections reaches the correction threshold and there are still curve segments that do not meet the constraints, curve correction is stopped; if the initial path point sequence still meets the connection path verification conditions, the initial path point sequence is output as a 3D global path, with an initial path rollback flag added.

[0296] When the initial path point sequence no longer meets the path feasibility detection requirements due to map updates or changes in obstacle distances, the path smoothing and detection module feeds back the node range corresponding to the failed section to the bidirectional search module. The bidirectional search module treats this node range as an invalid connection region in the current path planning task and continues to form new candidate connection pairs. The new initial path point sequence re-enters the path smoothing and detection module until a 3D global path is output or a "no feasible path" marker is formed.

[0297] The output 3D global path is arranged in order from the path start point to the path end point, and includes the 3D coordinates of multiple path points and the directional information between adjacent path points. The 3D global path is sent to the navigation controller of the ground mobile robot for subsequent trajectory generation and path tracking. When the 3D point cloud map, the path start point, or the path end point changes, the system restarts the point cloud preprocessing module and executes a new round of path planning according to the module connection order.

Claims

1. A bidirectional search path planning method based on three-dimensional point cloud terrain assessment, characterized in that, include: S100: Obtain a 3D point cloud map, path start point, path end point, and robot travel parameters; separate terrain points and obstacle points on the 3D point cloud map; and generate candidate node sets and obstacle point sets based on the terrain points; specifically including: The three-dimensional point cloud map is subjected to voxel downsampling and statistical outlier filtering to generate a preprocessed point cloud; Based on the normal vector and elevation difference of the preprocessed point cloud, terrain points and obstacle points are separated; A spatial index is constructed based on the terrain points, and the terrain points are spatially sampled according to the search step size. Node sampling units corresponding to the search step size are established on the horizontal projection plane of the world coordinate system. In each sampling unit, terrain points that are close to the center of the unit and have effective elevations are selected as candidate terrain nodes. For cases where there are multiple elevation layers at the same horizontal position, candidate terrain nodes corresponding to different elevation layers are retained respectively, generating a candidate node set and an obstacle point set. The robot passage parameters include the robot's equivalent safety radius, maximum allowable slope, maximum allowable terrain undulation, search step size, terrain passability threshold, and obstacle expansion radius. S200. Based on the candidate node set, establish a local point cloud neighborhood and fit a local terrain plane to generate local terrain features; determine the passage cost based on the local terrain features, use the passage cost to filter effective nodes and establish adjacency relationships, and generate a three-dimensional search graph; the establishment of the local point cloud neighborhood includes: Using candidate terrain nodes as the center, query terrain points from the spatial index according to the local neighborhood scale; Point cloud missing markers are generated based on the number of neighboring points in the local point cloud neighborhood and the point cloud coverage. Candidate terrain nodes with missing point cloud markers are marked as impassable points, and local terrain planes are fitted to the remaining candidate terrain nodes. The process of establishing a local point cloud neighborhood based on a candidate node set and fitting a local terrain plane includes: The local neighborhood scale is configured in association with the robot's footprint size and search step size, and the neighborhood range covers the terrain area that the robot may come into contact with when it is at that node. The horizontal projection area of ​​the local neighborhood is divided into multiple directional sub-regions. The number of directional sub-regions containing effective shape points is counted and the ratio to the total number of directional sub-regions is calculated to obtain the point cloud coverage rate. When the number of neighborhood points is lower than the threshold for the number of neighborhood points or the point cloud coverage rate is lower than the threshold for the point cloud coverage rate, the corresponding candidate terrain nodes are marked with point cloud missing markers and marked as impassable points. For candidate terrain nodes that are not marked as impassable, calculate the centroid of each terrain point in the local point cloud neighborhood, form a data matrix with the coordinates of each terrain point relative to the centroid, obtain the normal vector of the local terrain plane through singular value decomposition, and use the centroid as a reference point on the local terrain plane. The generation of local terrain features includes: The slope is calculated based on the angle between the normal vector of the local terrain plane and the Z-axis of the world coordinate system; the slope of a horizontal ground is close to zero. Flatness is calculated based on the root mean square of the vertical distance from the terrain point in the local point cloud neighborhood to the local terrain plane, and the root mean square is used as the terrain undulation value. Point cloud sparsity is calculated by weighting the ratio of the number of directions without point cloud coverage to the total number of directional sub-regions and the void ratio. Edge features are extracted based on the elevation difference between high-order and low-order quantiles in the local point cloud neighborhood. The passage cost is determined based on local terrain features, including: slope, flatness, point cloud sparsity, and edge features are normalized according to the maximum allowable slope, the flatness reference value corresponding to the maximum allowable terrain undulation, the sparsity upper limit of 1, and the maximum allowable terrain undulation, respectively. The four normalized features are then weighted and fused according to preset weights to form the passage cost corresponding to the candidate terrain node; the weights are configured according to the robot's structural form and passage capability. The step of using the passage cost to filter valid nodes and establish adjacency relationships includes: When the passage cost of a candidate terrain node exceeds the terrain passability threshold, the corresponding candidate terrain node is marked as an impassable point, and the remaining candidate terrain nodes are determined as valid nodes. The passage cost maintains a correspondence with the valid nodes after the node screening is completed, and is used as a terrain component of the cumulative cost in subsequent steps. For the current valid node, query other valid nodes whose three-dimensional Euclidean distance is within the allowed range of the search step size from the spatial index; for each node to be connected, compare the elevation difference between the two nodes and the angle between the local terrain plane normal vectors. If the elevation difference exceeds the maximum allowed terrain undulation or the angle between the normal vectors exceeds the terrain continuity threshold, do not establish a search adjacent edge. For the straight line connection between two valid nodes, set connection sampling points at a sampling interval no greater than the search step size, query the passage cost of valid nodes near each connection sampling point, and do not establish search adjacent edges when the connection sampling point cannot be mapped to a valid node or its passage cost exceeds the terrain passability threshold. S300. Based on the three-dimensional search map and obstacle point set, construct the obstacle distance distribution, expand the forward search end and reverse search end from the path start point and path end point, include the passage cost and obstacle distance cost in the cumulative cost, and generate candidate connection pairs. S400: Verify the connection path based on the node-level terrain accessibility, minimum obstacle distance, and turning change of the connection segment in the candidate connection pair; if the verification is successful, track the forward parent node and the reverse parent node to generate an initial path point sequence; generate a smooth path based on the initial path point sequence and verify it to obtain curve segments that do not meet the constraints; backtrack the path nodes or insert path nodes based on the curve segments that do not meet the constraints, regenerate and output the three-dimensional global path.

2. The method according to claim 1, characterized in that, In S300, the generation of the obstacle distance cost and cumulative cost includes: Query the distance from a valid node to the nearest obstacle point; When the distance to the obstacle is not greater than the robot's equivalent safe radius, the corresponding valid node will be deleted from the 3D search graph; When the obstacle distance is greater than the robot's equivalent safety radius and less than the obstacle's expansion radius, a continuously decaying obstacle distance cost is generated based on the obstacle distance. The geometric movement cost is generated based on the three-dimensional Euclidean distance between adjacent valid nodes, and the turning penalty is generated based on the turning angle between the parent node direction and the candidate node direction. The geometric movement cost, passage cost, obstacle distance cost and turning penalty are included in the cumulative cost.

3. The method according to claim 2, characterized in that, The extensions to the forward and reverse search ends include: Establish a forward open node set and a forward closed node set based on the valid nodes corresponding to the starting point of the path, and establish a reverse open node set and a reverse closed node set based on the valid nodes corresponding to the ending point of the path; A composite evaluation value is generated based on the cumulative cost and the heuristic cost. Nodes to be expanded are selected from the forward open node set and the reverse open node set according to the composite evaluation value. The heuristic cost is generated by performing an exponential function mapping on the 3D distance from the node to be expanded to the target node. Record the parent node of each node to be expanded, and generate candidate connection pairs based on the spatial distance between the forward search end and the reverse search end.

4. The method according to claim 3, characterized in that, In S400, the connection path verification includes: When the spatial distance between a node in the forward search end and a node in the reverse search end is not greater than the connection distance threshold, multiple verification sampling points are set on the corresponding connection segment. Query the node-level terrain accessibility and obstacle distance corresponding to each verification sampling point, and calculate the turning angle between the forward path direction, the connecting segment direction, and the reverse path direction; When the node-level terrain accessibility corresponding to each verification sampling point does not exceed the terrain accessibility threshold, the obstacle distance is not less than the robot's equivalent safety radius, and the turning angle does not exceed the turning angle threshold, a connection path verification result is generated. If a connection segment fails the verification, continue to expand the forward or reverse search end; if a connection segment passes the verification, trace the forward and reverse parent nodes respectively and concatenate them to generate the initial path point sequence.

5. The method according to claim 4, characterized in that, The generation and verification of the smooth path includes: Perform node-level terrain accessibility and obstacle distance verification on the connecting segments between adjacent non-contiguous nodes in the initial path point sequence, delete the intermediate nodes between adjacent non-contiguous nodes that pass the verification, and generate a simplified path node sequence. Using the simplified path node sequence as path nodes, a cubic B-spline curve is generated to obtain a smooth path; Multiple path sampling points are set on the smooth path, and curve segments that do not meet the constraints are determined based on the node-level terrain accessibility and obstacle distance corresponding to each path sampling point. Restore the initial path point corresponding to the curve segment that does not meet the constraint, or insert a path node into the curve segment that does not meet the constraint to regenerate a smooth path; When the regenerated smooth path passes the path feasibility test, the three-dimensional global path is output; when the number of path corrections reaches the correction threshold and the smooth path fails the path feasibility test, the initial path point sequence is output.

6. A bidirectional search path planning system based on three-dimensional point cloud terrain assessment, applied to the method of any one of claims 1 to 5, characterized in that, include: The module includes a point cloud preprocessing module, a terrain assessment and search map construction module, a bidirectional search module, a connection path verification and initial path generation module, and a path smoothing and detection module.

Images