Robot path planning method and system, robot and readable storage medium

Abstract

The invention provides a robot path planning method and system, a robot and a readable storage medium. The method specifically comprises the following steps: traversing neighbor nodes communicated with a starting node serving as an extension node, and determining a first path cost; determining whether to update the optimal path cost of the neighbor node based on the first path cost; if yes, determining the extension node as a father node of the neighbor node; on the basis of the second path cost, selecting a new extension node for traversal until the target node determines a father node of the target node; based on the father node relationship, planning an executable path of the robot in the three-dimensional topological graph; the first path cost is associated with at least one of a height difference, roughness and a trafficability score between the nodes; the second path cost is associated with at least one of a height difference between nodes, a trafficability score. According to the trajectory planning mode, the trajectory planning requirement of the robot in the three-dimensional space can be met, the trajectory planning problem is simplified, and the solving efficiency and precision are higher.

Description

Technical Field

[0001] This invention belongs to the field of robot navigation technology, specifically relating to a robot path planning method, system, robot, and readable storage medium. Background Technology

[0002] Autonomous navigation of mobile robots in complex, unstructured environments is one of the core challenges in the field of robotics. Path planning, as a key component of autonomous navigation, aims to find a safe and efficient trajectory for the robot from its starting point to its destination within a given environmental map.

[0003] The aforementioned traditional methods have gradually revealed numerous limitations when dealing with mobile platforms such as quadruped robots that have specific requirements for terrain adaptability. First, the geometrically shortest path is not equivalent to the robot's optimal movement path. Traditional methods neglect the physical properties of the terrain, which are crucial to the robot's motion stability, energy consumption, and safety. This may result in a planned path that, while having the shortest geometric distance, requires the robot to frequently climb steep slopes, traverse impassable vegetated areas, or walk on severely bumpy ground, significantly increasing energy consumption and potentially causing robot instability, slippage, or even mission failure.

[0004] Secondly, in the subsequent processing of path planning, the existing solution fails to fully consider the "executability" of the path. If executed directly, the robot may need to make drastic posture adjustments at each path point, resulting in disjointed movement and low efficiency.

[0005] Therefore, in order to address the above-mentioned technical problems, it is necessary to provide a robot path planning method, system, robot, and readable storage medium.

[0006] The information disclosed in this background section is intended only to enhance the understanding of the overall background of the invention and should not be construed as an admission or in any way implying that the information constitutes prior art known to those skilled in the art. Summary of the Invention

[0007] The purpose of this invention is to provide a robot path planning method, system, robot, and readable storage medium, which solves the problem that path planning is difficult to match the actual needs of robots.

[0008] To achieve the above objectives, a specific embodiment of the present invention provides the following technical solution:

[0009] In a first aspect, the present invention provides a robot path planning method, comprising:

[0010] The starting node and target node of the robot are determined in a three-dimensional topology graph, which includes multiple topology nodes that allow the robot to move.

[0011] Using the starting node as the extended node, traverse the neighboring nodes connected to it to determine the first path cost, where the first path cost represents the cost of reaching the neighboring node from the starting node via the extended node.

[0012] Determine whether to update the optimal path cost of the neighbor node based on the first path cost; if so, determine the extended node as the parent node of the neighbor node.

[0013] Based on the second path cost, among the candidate topology nodes that have a parent node but have not yet been used as extension nodes, a new extension node is selected for traversal until the target node determines its parent node. Here, the second path cost represents the cost of reaching the target node from the candidate topology nodes.

[0014] Based on the parent node relationship, plan the robot's executable path in the three-dimensional topology graph;

[0015] Wherein, the first path cost is associated with at least one of the height difference between nodes, roughness, and drivability score, and / or, the second path cost is associated with at least one of the height difference between nodes and drivability score.

[0016] In one or more embodiments of the present invention, when selecting a new expansion node from candidate topology nodes that have a parent node but have not yet been selected as expansion nodes, the method specifically includes:

[0017] Based on the optimal path cost and the second path cost of the candidate topology node, determine the comprehensive path cost corresponding to the candidate topology node;

[0018] Based on the comprehensive path cost, a new expansion node is selected from the candidate topology nodes.

[0019] In one or more embodiments of the present invention, the method further includes calculating the second path cost, specifically including:

[0020] Based on the drivability scores of the neighboring nodes and the target node, a drivability penalty is determined, and the drivability penalty is inversely correlated with the linear combination of the drivability scores of the neighboring nodes and the target node;

[0021] The first sub-cost is determined based on a linear combination of the drivability penalty and the height difference between the neighboring node and the target node;

[0022] The second path cost is determined based on the product of the first sub-cost and the Euclidean distance between nodes.

[0023] In one or more embodiments of the present invention, the method further includes:

[0024] Obtain the desired robot start-point and end-point coordinates, and map the start-point and end-point coordinates to the start node and target node in the three-dimensional topology graph.

[0025] In one or more embodiments of the present invention, mapping the starting point coordinates and the ending point coordinates to the starting node and the target node in the three-dimensional topology graph includes:

[0026] In the three-dimensional topology graph, determine the topology node closest to the starting point coordinates and the topology node closest to the ending point coordinates;

[0027] The starting point coordinates are mapped to the topology node that is closest to the starting point coordinates, and the ending point coordinates are mapped to the topology node that is closest to the ending point coordinates.

[0028] In one or more embodiments of the present invention, when traversing neighboring nodes connected to the extended node, the method further includes: skipping neighboring nodes that have been elected as extended nodes.

[0029] In one or more embodiments of the present invention, determining whether to update the optimal path cost of the neighboring node based on the first path cost specifically includes:

[0030] Determine whether the cost of the first path is less than the optimal path cost of the neighboring node, or whether the optimal path cost of the neighboring node is empty; if so,

[0031] The optimal path cost of the neighboring node is updated using the first path cost.

[0032] In one or more embodiments of the present invention, determining the first path cost includes:

[0033] Based on the horizontal distance between the extended node and its neighboring nodes, a basic distance cost is calculated, and the basic distance cost is positively correlated with the horizontal distance.

[0034] Based on the product of the drivability scores of the extended node and its neighboring nodes and the horizontal distance between the extended node and its neighboring nodes, the drivability cost is determined; the drivability cost is inversely correlated with the linear combination of the drivability scores of the extended node and its neighboring nodes, and positively correlated with the horizontal distance between the extended node and its neighboring nodes.

[0035] The climbing cost is determined based on the height difference between the extended node and its neighboring nodes; the climbing cost is positively correlated with the height difference.

[0036] Determine the mean roughness score of the extended node and its neighboring nodes, and determine the roughness cost based on the product of the mean score and the horizontal distance.

[0037] The first path cost of the neighboring node is determined based on at least one of the basic distance cost, the drivability cost, the hill-climbing cost, and the roughness cost, as well as the optimal path cost of the extended node.

[0038] In one or more embodiments of the present invention, the method further includes:

[0039] Determine if the number of visited extended nodes exceeds the preset upper limit for the number of extended nodes, or if the path planning time exceeds the preset time threshold; if so...

[0040] Terminate route planning and report an alarm signal.

[0041] In one or more embodiments of the present invention, the step of planning the executable path of the robot in the three-dimensional topology map includes:

[0042] The optimal path node sequence is formed by tracing back from the target node along the recorded sequence of parent nodes to the starting node.

[0043] Based on the three-dimensional position information of each topological node in the optimal path node sequence, the corresponding path point positions are generated.

[0044] For each path node in the sequence except the target node, its horizontal yaw angle is calculated based on the direction vector pointing from the path node to the next path node; for the final path point of the sequence, a preset target orientation is adopted.

[0045] The path point poses containing path point positions and orientation information are output sequentially to form the executable path of the robot.

[0046] In a second aspect, the present invention provides a robot path planning system, comprising:

[0047] The first determining module is used to determine the starting node and target node of the robot in a three-dimensional topology map, wherein the three-dimensional topology map includes multiple topology nodes that the robot can move.

[0048] The traversal module is used to traverse the neighboring nodes connected to the starting node as the extended node, and determine the first path cost, wherein the first path cost represents the cost of reaching the neighboring node from the starting node via the extended node.

[0049] The second determining module is used to determine whether to update the optimal path cost of the neighbor node based on the first path cost; if so, the extended node is determined as the parent node of the neighbor node.

[0050] The selection module is used to select a new expansion node from the candidate topology nodes that have a parent node but have not yet been used as expansion nodes, based on the second path cost, and traverse until the target node determines its parent node, wherein the second path cost represents the cost of reaching the target node from the candidate topology nodes.

[0051] The planning module is used to plan the robot's executable path in the three-dimensional topology graph based on the parent node relationship;

[0052] Wherein, the first path cost is associated with at least one of the height difference between nodes, roughness, and drivability score, and / or, the second path cost is associated with at least one of the height difference between nodes and drivability score.

[0053] Thirdly, the present invention provides a robot comprising: a memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the robot path planning method by executing the computer instructions.

[0054] Fourthly, the present invention provides a computer-readable storage medium storing computer instructions for causing a computer to execute the robot path planning method.

[0055] Compared with existing technologies, the robot path planning method provided by this invention ensures that path planning can be performed in three-dimensional space by applying a three-dimensional topology graph, enabling it to effectively handle three-dimensional terrain variations in the real world. Furthermore, by combining the selection and traversal of extended nodes, the parent node with the lowest cost is generated. Finally, the optimal path is formed by backtracking based on the parent node. This reduces planning time and computational pressure. It is applicable to environmental performance indicators such as drivability and roughness, allowing the robot to adapt to complex scenarios such as uneven terrain and multi-story buildings, generating paths that better match actual terrain characteristics, thereby improving the feasibility and safety of the path. Attached Figure Description

[0056] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0057] Figure 1 This is a flowchart of a robot path planning method in one embodiment of the present invention;

[0058] Figure 2 This is a structural block diagram of a robot path planning system according to another embodiment of the present invention;

[0059] Figure 3 This is a structural block diagram of a robot according to one embodiment of the present invention;

[0060] Figure 4 This is a schematic diagram of path planning.

[0061] Figure 5 This is a comparative schematic diagram of another path planning method;

[0062] Figure 6 This is a schematic diagram of path planning in one embodiment of the present invention. Detailed Implementation

[0063] To enable those skilled in the art to better understand the technical solutions of this invention, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this invention.

[0064] Unless otherwise expressly stated, throughout the specification and claims, the term "comprising" or its variations such as "including" or "comprises" shall be understood to include the stated elements or components without excluding other elements or other components.

[0065] In existing robot path planning schemes, common techniques are mainly based on two-dimensional grids or continuous coordinate spaces. Specifically, this includes using Dijkstra's algorithm for path search on a two-dimensional plane, or directly exploring feasible paths through random sampling. However, these methods generally ignore the three-dimensional structural features of the environment itself and the actual motion constraints of the robot, thus exhibiting significant shortcomings when dealing with complex terrain. These shortcomings are mainly manifested in the following ways: the generated paths are often infeasible in real-world environments; the search efficiency is low, leading to a serious waste of computational resources; and the quality of the final path is poor, failing to meet the requirements for efficient and stable robot motion.

[0066] To address the aforementioned problems, the inventors of this invention propose a novel technical approach: First, a 3D topological graph containing multiple topological nodes is constructed to accurately model the environmental space in a structured manner. Then, the starting and ending coordinates of the path planning are mapped to the starting and target nodes in this graph, respectively. Based on this, an improved search algorithm is used to explore paths on the topological graph. This algorithm also innovatively introduces a comprehensive cost calculation mechanism based on factors such as drivability scoring, height difference, and terrain roughness. This ensures high search efficiency while generating high-quality, feasible paths that adapt to complex 3D terrain features and conform to the actual motion characteristics of the robot.

[0067] Please refer to Figure 1 The diagram shown is a flowchart of robot path planning according to an embodiment of the present invention. The robot path planning method specifically includes the following steps:

[0068] S101: Determine the starting node and target node of the robot in a three-dimensional topology graph, wherein the three-dimensional topology graph includes multiple topology nodes that the robot can move through;

[0069] It should be noted that, in one embodiment of the present invention, a topology node can be an abstract description of a key location point in three-dimensional space that allows the robot to walk or move. Each topology node can contain corresponding location information; furthermore, based on different user configurations, it can also serve as a data structure carrying various environmental semantic information.

[0070] Topological nodes can establish connectivity, meaning each node stores information about its directly connected "neighboring nodes," defining the next set of feasible nodes the robot can directly move from its current node. The network graph structure formed by these topological nodes and their connections is called a three-dimensional topological graph. This graph structure is essentially a way of discretizing and abstracting a continuous three-dimensional space, and endowing it with semantic information—a method of environmental modeling. Because the three-dimensional topological graph explicitly represents the locations of nodes accessible to the robot in the workspace and their connections, it can be used to respond to the robot's path planning requests and assist in generating the optimal motion path that the robot can execute.

[0071] In one embodiment of the present invention, the user's desired start-point and end-point coordinates for robot movement are typically included in the path planning request. Based on this, the path planning method of the present invention, in determining the robot's starting and target nodes in a three-dimensional topology map, may include the following steps: First, in response to the received path planning request, the desired start-point and end-point coordinates specified therein are obtained; then, the start-point coordinates are mapped to the corresponding nodes in the three-dimensional topology map as the starting node, and the end-point coordinates are mapped to the corresponding nodes in the three-dimensional topology map as the target node.

[0072] Understandably, before performing coordinate mapping, it can be determined whether the starting point coordinates and ending point coordinates are in the same coordinate system as the 3D topology map. If they are in the same coordinate system, the mapping process can proceed directly; if the coordinate systems are inconsistent, coordinate transformation can be performed before mapping to unify them to the coordinate system of the topology map, so as to avoid deviations in subsequent path planning due to coordinate system differences.

[0073] Furthermore, in an exemplary embodiment of the present invention, mapping the starting point coordinates and the ending point coordinates to a starting node and a target node in a three-dimensional topology graph may include: creating new nodes at corresponding positions in the three-dimensional topology graph based on the starting point coordinates and the ending point coordinates; defining the node created based on the starting point coordinates as the starting node, and defining the node created based on the ending point coordinates as the target node. This accurately maps the actual coordinates to the topology structure.

[0074] Understandably, this implementation method faithfully reflects the actual coordinate position, resulting in more accurate path planning and greater flexibility. However, adding nodes may alter the existing topology, increasing the complexity of graph maintenance; if new nodes are too close to existing nodes, it may also lead to topological redundancy. Therefore, this implementation method is typically suitable for robot systems with high path accuracy requirements, dynamically changing working environments, or those that allow for online topology updates.

[0075] Based on the advantages and disadvantages of the above embodiments, the present invention also provides another exemplary embodiment, including: determining the topological node closest to the starting point coordinates and the topological node closest to the ending point coordinates in the three-dimensional topology graph; mapping the starting point coordinates to the topological node closest to the starting point coordinates, and mapping the ending point coordinates to the topological node closest to the ending point coordinates.

[0076] This implementation method does not alter the original topology structure, is simple to implement, has low computational overhead, and helps maintain system stability and real-time performance. However, the mapping result depends on the distribution of existing nodes. If the node density is insufficient, the starting / ending point may be mapped to a non-ideal location, introducing planning errors. It is suitable for scenarios with stable topology, reasonable node distribution, and high real-time requirements.

[0077] S102: Using the starting node as the extended node, traverse the neighboring nodes connected to it to determine the first path cost, wherein the first path cost represents the cost of reaching the neighboring node from the starting node via the extended node.

[0078] This invention traverses the topological nodes in a 3D topological graph using a search loop and evaluates the quality of a path based on the path cost from the starting point to each traversed node. Specifically, in this invention, path cost is a numerical metric used to quantify the quality of a path, representing the cost or loss incurred by the robot to move from the starting point to the corresponding point along a certain path. The lower the path cost, the higher the priority of the path.

[0079] The path cost can be based on a single factor or a combination of multiple factors; this invention does not impose any restrictions on this. As mentioned above, each topological node can contain corresponding location information; furthermore, based on different user configurations, it can also serve as a data structure carrying various environmental semantic information. Therefore, this invention creatively integrates environmental semantic information into path cost calculation. This invention does not impose any restrictions on the specific selection of environmental semantic information. For example, in one embodiment of this invention, the first path cost is associated with at least one of the following: the height difference between nodes, roughness, and drivability score.

[0080] It should be noted that environmental roughness can be a physical index that quantifies the degree of surface unevenness in a local area of ​​the ground, used to describe microscopic variations in undulation. High roughness often characterizes rugged and uneven ground, such as gravel beaches, cobblestone roads, areas with dense tree roots, and brick pavements; low roughness often characterizes smooth and flat surfaces, such as indoor floors and concrete pavements. In one embodiment, roughness can be calculated by analyzing and analyzing point cloud data from a 3D LiDAR or depth camera on a robot. A small local point cloud region is taken around a topological node. A fitting plane is used to approximate these points. The standard deviation or variance of the distance from each point to this fitting plane is calculated. The roughness value of each topological node is defined based on the value of the standard deviation or variance.

[0081] A node accessibility score can be a user-defined value for each node, used to assess whether a topological node's location is easy and safe for a robot to traverse. For example, using a staircase as an example, even if the robot can move along the edges of the staircase without collision, using the center of the staircase as a landing point is still safer than using the edges. Therefore, in a 3D topology graph, the accessibility score of a topological node located in the center of the staircase can be defined as greater than the accessibility score of a topological node located at the edge of the staircase. In one implementation, the accessibility of a node can be positively correlated with the node density within a certain area in the 3D topology graph and inversely correlated with the distance to the nearest obstacle.

[0082] Incorporating the aforementioned semantic information into the calculation of the first path cost can improve the accuracy and reliability of the final path planning. Take climb distance and accessibility scoring as an example. (Refer to...) Figures 4-6These are, respectively, the robot executable path formed by removing the height difference constraint between nodes, the robot executable path formed by removing the drivability score constraint, and the robot executable path formed by applying the path planning method of this invention. It is clear that... Figure 4 The robot goes directly up the stairs, a path that could easily cause collisions during actual use. Figure 5 The resulting path places the clock close to the edge of the stairs, significantly compromising safety. Conversely, Figure 6 By applying the path planning method of this invention and adding environmental feature constraints, a safer and more reliable executable path for the robot is formed.

[0083] Specifically, in an exemplary embodiment, determining the first path cost may include: calculating a base distance cost based on the horizontal distance between the extended node and its neighboring nodes, wherein the base distance cost is positively correlated with the horizontal distance; determining a traversability cost based on the traversability scores of the extended node and its neighboring nodes and the product of the horizontal distances between the extended node and its neighboring nodes; wherein the traversability cost is inversely correlated with a linear combination of the traversability scores of the extended node and its neighboring nodes and positively correlated with the horizontal distances between the extended node and its neighboring nodes; determining a ramp cost based on the height difference between the extended node and its neighboring nodes; wherein the ramp cost is positively correlated with the height difference; determining the mean roughness score of the extended node and its neighboring nodes, and determining a roughness cost based on the product of the mean score and the horizontal distances; and determining the first path cost of the neighboring node based on at least one of the base distance cost, the traversability cost, the ramp cost, the roughness cost, and the optimal path cost of the extended node.

[0084] For example, in one specific embodiment, calculating the first path cost from the starting node to the neighboring node via the extended node includes: obtaining the horizontal distance between the extended node and the neighboring node. and height difference Based on the horizontal distance, the basic distance cost, drivability cost, and climbing cost between the extended node and the neighboring node are calculated as follows:

[0085]

[0086]

[0087]

[0088] in, , , This is a preset constant; These are the normalization coefficients; Score the drivability of the neighboring nodes. The drivability score is given to the starting node;

[0089] Furthermore, based on the preset roughness scores for each node, the average roughness score of the extended node and its neighboring nodes is obtained. And calculate the roughness cost as follows:

[0090]

[0091] The cost of the first path, calculated by combining the aforementioned basic distance cost, drivability cost, climbing cost, and roughness cost, is as follows:

[0092]

[0093] S103: Determine whether to update the optimal path cost of the neighbor node based on the first path cost; if so, determine the extended node as the parent node of the neighbor node;

[0094] During path search, the first path cost from the starting node to its neighboring node via the current expanding node is calculated. The core objective is to dynamically determine the optimal path from the starting node to that neighboring node. Due to the graph structure of path search, the same topological node may be traversed multiple times by different expanding nodes; that is, the same node may be repeatedly visited as a node on multiple potential paths. Each time it is visited, the system calculates a first path cost based on the current path. Therefore, the actual optimal path to that neighboring node, corresponding to the path with the minimum cumulative cost among all possible paths, is specifically represented by the minimum value of the first path cost calculated across all visits.

[0095] The essence of path planning is to find a globally optimal path from the starting point to the destination among many possible paths. However, the algorithm cannot know which path is optimal from the outset; it must gradually build global knowledge through local exploration. To achieve the identification and updating of the optimal path, this invention provides a corresponding data management mechanism. Specifically, the system is equipped with a storage unit to record in real time the known optimal path cost of each traversed neighbor node and to record the extended node corresponding to the optimal path cost as the parent node of that neighbor node. Simultaneously, the system presets explicit update trigger conditions to ensure that the path information of nodes can be updated in a timely and controllable manner when a better path is discovered.

[0096] In an exemplary embodiment, the update triggering condition may be configured as follows: the calculated first path cost is less than the optimal path cost already recorded by the neighboring node; and / or the neighboring node has not yet been recorded with any optimal path cost, i.e., it is the first node to be visited.

[0097] When any of the above conditions are met, the system performs an update operation: updating the optimal path cost of the neighboring node to the currently calculated first path cost, and updating its parent node to the currently expanding node. Through this mechanism, the system ensures that each node always maintains the optimal path information from the starting node, thus laying the foundation for the final reconstruction of the globally optimal path. Since the path cost calculation in this invention is not limited to the minimum value of Euclidean distance, the update at this point is not merely a switch from a "long path" to a "short path," but also an intelligent optimization from a "difficult path" to an "easy path." This mechanism ensures that the path planned for the robot in this invention can simultaneously consider its motion stability, safety, and energy efficiency as the terrain-optimal path, and not just the geometrically shortest path.

[0098] S104: Based on the second path cost, among the candidate topology nodes that have a parent node but have not yet been used as expansion nodes, select a new expansion node for traversal until the target node determines its parent node.

[0099] Understandably, the selection strategy for expanding nodes directly impacts the efficiency of the algorithm and the quality of the final path during path search. Randomly selecting expanding nodes can easily lead to the algorithm getting stuck in localized regions or invalid branches. For example, it might select a topological node that is not connected to the current path backbone or has not yet established a valid parent-node relationship, causing the path cost to be unable to be calculated cumulatively along the starting node, thus disrupting the logical continuity of the search process.

[0100] To ensure the path search has a clear direction and continuity, this invention imposes explicit constraints on the selection range of expansion nodes. Specifically, new expansion nodes should be selected from topological nodes that have already recorded parent nodes but have not yet been expanded themselves. These nodes constitute the "effective frontier" for the algorithm's continuous exploration, where each node has established a definite path connection with the starting node through its parent node pointer, thus ensuring that each expansion step is based on existing optimal path information.

[0101] Furthermore, it should be noted that the second path cost in this invention represents the cost of reaching the target node from the candidate topology node; similar to the first path cost, the second path cost in this invention can also be calculated by integrating various environmental features. In one embodiment, the second path cost can be associated with any one or more of the following: First, the Euclidean distance between the target nodes, that is, the straight-line distance between the current node and the target node in three-dimensional space, used as a basic geometric metric in the estimated cost; second, the drivability score, which is a value quantified based on environmental perception data, used to characterize the difficulty of the terrain surface for robot movement, with a higher score indicating less resistance to passage; third, the height difference, defined as the absolute elevation difference between the current node and the target node in the vertical direction, used to reflect the scale of slope change between the two locations.

[0102] In an exemplary embodiment, calculating the second path cost may include: determining a traversability penalty based on the traversability scores of the neighboring nodes and the target node, wherein the traversability penalty is inversely correlated with a linear combination of the traversability scores of the neighboring nodes and the target node; determining a first sub-cost based on a linear combination of the traversability penalty and the height difference between the neighboring nodes and the target node; and determining the second path cost based on the product of the first sub-cost and the Euclidean distance between the nodes.

[0103] For example, in one specific embodiment, calculating the second path cost includes: obtaining the Euclidean distance from the neighbor node to the target node. and height difference Based on the pre-defined traversability scores for each topological node, calculate the traversability penalty from the neighboring node to the target node; and based on the Euclidean distance... Height difference and accessibility penalties The estimated costs are calculated as follows:

[0104]

[0105]

[0106] in, Score the drivability of the neighboring nodes. Score the drivability of the target node. These are the normalization coefficients; , , , This is a preset constant.

[0107] Understandably, if node selection is based solely on the cost of the second path, the algorithm will completely ignore the actual cumulative cost from the starting node to the current node. This strategy causes the algorithm to blindly pursue nodes that seem heuristically close to the goal, without rationally weighing the costs already incurred, potentially leading the search into local optima. In scenarios with complex terrain obstacles or U-shaped paths, the algorithm is easily misled towards directions with extremely high actual travel costs, causing it to oscillate repeatedly before local obstacles and fail to converge to the global optimum. Furthermore, the lack of consideration for historical path costs results in ineffective consumption of computational resources, severely compromising the completeness and optimality of the path planning algorithm.

[0108] Based on this, in an exemplary embodiment of the present invention, when selecting a new expansion node from candidate topology nodes that have a parent node but have not yet been selected as expansion nodes, the method specifically includes: determining the comprehensive path cost corresponding to the candidate topology node based on the optimal path cost and the second path cost of the candidate topology node; and selecting a new expansion node from the candidate topology nodes based on the comprehensive path cost. In one implementation, the comprehensive path cost can be a weighted sum of the optimal path cost and the second path cost, and its weight value can be dynamically adjusted based on the actual usage environment; this embodiment of the present invention does not impose any limitations on this.

[0109] The core purpose of expanding node selection based on the second path cost is to provide target-node-oriented guidance for the search process. The second path cost is a forward-looking estimate of the potential cost required to reach the target node from the current candidate nodes. Correspondingly, the optimal path cost is the actual minimum cumulative cost from the starting node to the current node, reflecting the historical cost in path searching. By combining "actual costs already incurred" with "predictions of future costs," the system can achieve an effective balance between current optimality and expected future optimality during the search process. This avoids nodes with low historical costs but deviating from the target direction and limited potential, as well as nodes that are close to the target but have extremely high actual travel costs and bleak prospects. Thus, the search process is upgraded from traditional mechanical traversal to an intelligent decision-making process with direction awareness and cost sensitivity. Ultimately, this ensures that in complex 3D terrain, a path can be found quickly, and the safest, most stable, and most energy-efficient high-quality path for the robot can be found. This not only guarantees the real-time performance of path searching but also ensures the comprehensive optimization of the output path in terms of robot motion safety, stability, and energy efficiency.

[0110] It should also be noted that, based on the above implementation method, when a node is selected from the open list as an expansion node, it means that the algorithm has already determined that the optimal path from the starting node to that node has been found. That is, even if the node is visited again, it is impossible to find a lower-cost path, which would instead waste computational resources. Therefore, in this embodiment of the invention, to avoid invalid computation and potential infinite loops, when selecting expansion nodes by estimating the cost and the cost of the optimal path, and traversing the neighboring nodes connected to the expansion node, neighboring nodes that were previously elected as expansion nodes can be skipped.

[0111] Furthermore, in one embodiment of the present invention, to further enhance the robustness and system controllability of the path planning process, an upper limit on the number of expanded nodes and a time threshold can be set to achieve proactive management of the search process. The upper limit on the number of expanded nodes limits the maximum number of nodes the algorithm can explore in the topology graph; the time threshold limits the maximum execution time of the path planning task. During the search process, once it is detected that the number of currently visited expanded nodes exceeds the preset upper limit, or the total time for path planning has reached or exceeded the set time threshold, the system will immediately terminate the current search process.

[0112] The interruption mechanism, through preset resource boundaries, effectively prevents the algorithm from exhausting computing resources or experiencing excessively high response latency in extremely complex or inaccessible environments. After the planning process is aborted, the system automatically generates and reports an alarm signal. This signal not only indicates the planning failure status but also provides crucial diagnostic information to the upper-level control system, enabling it to take appropriate measures based on the actual situation, such as relaxing constraints, switching to backup strategies, or retrying after environmental changes. This significantly improves the adaptability and reliability of the entire navigation system in real-world application scenarios.

[0113] S105: Based on the parent node relationship, plan the executable path of the robot in the three-dimensional topology graph;

[0114] As mentioned above, a core rule of this invention is that when the optimal path cost of a node is updated, its parent node is also synchronously updated to the extended node that currently brings a lower path cost. In other words, any topological node whose parent node is recorded during the search process uniquely represents the previous step of the currently known optimal path from the starting point to that node. Tracing back the parent node sequence from the target node is essentially deducing the complete globally optimal path by following this series of locally optimal decisions.

[0115] It should be noted that if the strategy of reselecting an extension node from the topological nodes that have recorded the parent node but have not yet been used as an extension node is adopted, then it is necessary to wait for all the neighbor nodes of the target node to be used as extension nodes and complete the traversal of the target node. At this time, the path backtracking of the target node along the parent node is the optimal path node sequence.

[0116] In another implementation, if the aforementioned strategy of selecting expansion nodes by combining estimated cost and optimal path cost is adopted, the algorithm can terminate when the target node is selected as an expansion node from the open list for the first time, because the path to it has been proven to be optimal. This is because the algorithm's selection criterion is the estimated total cost, and the design of the estimated cost function ensures that the estimate does not overestimate the actual cost. Therefore, when the target node is selected, there are no other unexplored potential paths that can reach the target node at a lower cost.

[0117] Furthermore, generating a robot-executable path based on the optimal path node sequence includes: generating corresponding path point positions based on the three-dimensional position information of each topological node in the optimal path node sequence; for each path node in the sequence other than the target node, calculating its horizontal yaw angle according to the direction vector pointing from the path node to the next path node; for the end path point of the sequence, adopting a preset target orientation; and sequentially outputting the path point poses containing path point positions and orientation information to form the robot-executable path.

[0118] Please refer to Figure 2 As shown, based on the same inventive concept as the aforementioned robot path planning method, one embodiment of the present invention provides a robot path planning system 200, including: a first determination module 201, a traversal module 202, a second determination module 203, a selection module 204, and a planning module 205.

[0119] Specifically, the first determining module 201 is used to determine the robot's starting node and target node in a three-dimensional topology graph, which includes multiple topology nodes that the robot can move through; the traversal module 202 is used to traverse the neighboring nodes connected to the starting node as an extension node, and determine a first path cost, wherein the first path cost represents the cost of reaching the neighboring node from the starting node via the extension node; the second determining module 203 is used to determine whether to update the optimal path cost of the neighboring node based on the first path cost; if so, the extension node is determined as the parent node of the neighboring node; the selection module 204 is used to select a new extension node from the candidate topology nodes that have a parent node but are not yet extension nodes, based on the second path cost, and traverse until the target node determines its parent node, wherein the second path cost represents the cost of reaching the target node from the candidate topology node; the planning module 205 is used to plan the robot's executable path in the three-dimensional topology graph based on the parent node relationship;

[0120] Wherein, the first path cost is associated with at least one of the height difference between nodes, roughness, and drivability score, and / or, the second path cost is associated with at least one of the height difference between nodes and drivability score.

[0121] Please refer to Figure 3 As shown, embodiments of the present invention also provide an electronic device 300, which includes at least one processor 301, a memory 302 (e.g., non-volatile memory), a memory 303, and a communication interface 304, wherein the at least one processor 301, the memory 302, the memory 303, and the communication interface 304 are connected together via an internal bus 305. The at least one processor 301 is used to invoke at least one program instruction stored or encoded in the memory 302, causing the at least one processor 301 to perform various operations and functions of the robot path planning methods described in the various embodiments of this specification.

[0122] In the embodiments of this specification, the electronic device 300 may include, but is not limited to: personal computer, server computer, workstation, desktop computer, laptop computer, notebook computer, mobile electronic device, smartphone, tablet computer, cellular phone, personal digital assistant (PDA), handheld device, messaging device, wearable electronic device, consumer electronic device, etc.

[0123] This invention also provides a computer-readable medium carrying computer-executable instructions. When executed by a processor, these instructions can be used to implement various operations and functions of the robot path planning methods described in the various embodiments of this specification.

[0124] The computer-readable medium in this invention can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this invention, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0125] In this invention, the computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. This propagated data signal may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. The computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to: wireless, wireline, optical fiber, RF, etc., or any suitable combination thereof.

[0126] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0127] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus, systems, and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0128] The foregoing description of specific exemplary embodiments of the invention is for illustrative and explanatory purposes. These descriptions are not intended to limit the invention to the precise forms disclosed, and it will be apparent that many changes and variations can be made in accordance with the foregoing teachings. The exemplary embodiments were chosen and described in order to explain the specific principles of the invention and its practical application, thereby enabling those skilled in the art to implement and utilize various different exemplary embodiments of the invention, as well as various different choices and variations. The scope of the invention is intended to be defined by the claims and their equivalents.

[0129] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0130] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A robot path planning method, characterized in that, include: The starting node and target node of the robot are determined in a three-dimensional topology graph, which includes multiple topology nodes that allow the robot to move. Using the starting node as the extended node, traverse the neighboring nodes connected to it to determine the first path cost, where the first path cost represents the cost of reaching the neighboring node from the starting node via the extended node. Determine whether to update the optimal path cost of the neighbor node based on the first path cost; if so, determine the extended node as the parent node of the neighbor node. Based on the second path cost, among the candidate topology nodes that have a parent node but have not yet been used as extension nodes, a new extension node is selected for traversal until the target node determines its parent node. Here, the second path cost represents the cost of reaching the target node from the candidate topology nodes. Based on the parent node relationship, plan the robot's executable path in the three-dimensional topology graph; Wherein, the first path cost is associated with at least one of the height difference between nodes, roughness, and drivability score, and / or, the second path cost is associated with at least one of the height difference between nodes and drivability score.

2. The robot path planning method according to claim 1, characterized in that, When selecting a new expansion node from candidate topology nodes that have a parent node but have not yet been selected as expansion nodes, the method specifically includes: Based on the optimal path cost and the second path cost of the candidate topology node, determine the comprehensive path cost corresponding to the candidate topology node; Based on the comprehensive path cost, a new expansion node is selected from the candidate topology nodes.

3. The robot path planning method according to claim 1, characterized in that, The method further includes calculating the cost of the second path, specifically including: Based on the drivability scores of the neighboring nodes and the target node, a drivability penalty is determined, and the drivability penalty is inversely correlated with the linear combination of the drivability scores of the neighboring nodes and the target node; The first sub-cost is determined based on a linear combination of the drivability penalty and the height difference between the neighboring node and the target node; The second path cost is determined based on the product of the first sub-cost and the Euclidean distance between nodes.

4. The robot path planning method according to claim 1, characterized in that, The method further includes: Obtain the desired robot start-point and end-point coordinates, and map the start-point and end-point coordinates to the start node and target node in the three-dimensional topology graph.

5. The robot path planning method according to claim 4, characterized in that, Mapping the starting point coordinates and ending point coordinates to the starting node and target node in the 3D topology graph includes: In the three-dimensional topology graph, determine the topology node closest to the starting point coordinates and the topology node closest to the ending point coordinates; The starting point coordinates are mapped to the topology node that is closest to the starting point coordinates, and the ending point coordinates are mapped to the topology node that is closest to the ending point coordinates.

6. The robot path planning method according to claim 1, characterized in that, When traversing neighboring nodes connected to the extended node, the method further includes: skipping neighboring nodes that were previously elected as extended nodes.

7. The robot path planning method according to claim 1, characterized in that, Determining whether to update the optimal path cost of the neighboring node based on the first path cost specifically includes: Determine whether the cost of the first path is less than the optimal path cost of the neighboring node, or whether the optimal path cost of the neighboring node is empty; if so, The optimal path cost of the neighboring node is updated using the first path cost.

8. The robot path planning method according to claim 1, characterized in that, Determining the cost of the first path includes: Based on the horizontal distance between the extended node and its neighboring nodes, a basic distance cost is calculated, and the basic distance cost is positively correlated with the horizontal distance. Based on the product of the drivability scores of the extended node and its neighboring nodes and the horizontal distance between the extended node and its neighboring nodes, the drivability cost is determined; the drivability cost is inversely correlated with the linear combination of the drivability scores of the extended node and its neighboring nodes, and positively correlated with the horizontal distance between the extended node and its neighboring nodes. The climbing cost is determined based on the height difference between the extended node and its neighboring nodes; the climbing cost is positively correlated with the height difference. Determine the mean roughness score of the extended node and its neighboring nodes, and determine the roughness cost based on the product of the mean score and the horizontal distance. The first path cost of the neighboring node is determined based on at least one of the basic distance cost, the drivability cost, the hill-climbing cost, and the roughness cost, as well as the optimal path cost of the extended node.

9. The robot path planning method according to claim 1, characterized in that, The method further includes: Determine if the number of visited extended nodes exceeds the preset upper limit for the number of extended nodes, or if the path planning time exceeds the preset time threshold; if so... Terminate route planning and report an alarm signal.

10. The robot path planning method according to claim 1, characterized in that, Planning the robot's executable path in the three-dimensional topology map includes: The optimal path node sequence is formed by tracing back from the target node along the recorded sequence of parent nodes to the starting node. Based on the three-dimensional position information of each topological node in the optimal path node sequence, the corresponding path point positions are generated. For each path node in the sequence except the target node, its horizontal yaw angle is calculated based on the direction vector pointing from the path node to the next path node; for the final path point of the sequence, a preset target orientation is adopted. The path point poses containing path point positions and orientation information are output sequentially to form the executable path of the robot.

11. A robot path planning system, characterized in that, include: The first determining module is used to determine the starting node and target node of the robot in a three-dimensional topology map, wherein the three-dimensional topology map includes multiple topology nodes that the robot can move. The traversal module is used to traverse the neighboring nodes connected to the starting node as the extended node, and determine the first path cost, wherein the first path cost represents the cost of reaching the neighboring node from the starting node via the extended node. The second determining module is used to determine whether to update the optimal path cost of the neighbor node based on the first path cost; if so, the extended node is determined as the parent node of the neighbor node. The selection module is used to select a new expansion node from the candidate topology nodes that have a parent node but have not yet been used as expansion nodes, based on the second path cost, and traverse until the target node determines its parent node, wherein the second path cost represents the cost of reaching the target node from the candidate topology nodes. The planning module is used to plan the robot's executable path in the three-dimensional topology graph based on the parent node relationship; Wherein, the first path cost is associated with at least one of the height difference between nodes, roughness, and drivability score, and / or, the second path cost is associated with at least one of the height difference between nodes and drivability score.

12. A robot, characterized in that, include: A memory and a processor are communicatively connected, the memory stores computer instructions, and the processor executes the robot path planning method according to any one of claims 1-10 by executing the computer instructions.

13. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions for causing the computer to execute the robot path planning method according to any one of claims 1-10.

Images