Method and device for planning safe motion of robot in uncertain environment under homotopy guidance

Abstract

The invention discloses a homotopy guided robot safe motion planning method and device in an uncertain environment, and relates to the field of robot trajectory planning, and the method comprises the steps: obtaining a generalized Voronoi diagram of a discretization space; the generalized Voronoi diagram comprises a plurality of nodes which are equidistant from the nearest obstacle, and the adjacent nodes are connected with each other; determining a space size attribute and a feature attribute of each node; determining feature nodes based on the space size attribute and the feature attribute of each node; constructing a feature graph based on the feature nodes; performing edge filtering on the feature map to obtain an updated feature map; performing feature map search on the updated feature map to obtain a region of interest; the feature map search comprises a starting position and a target position of a task; and performing non-uniform search based on the region of interest to obtain an optimal path. According to the method, the mobile robot can efficiently search the optimal and belief-feasible path in a complex environment.

Description

Technical Field

[0001] The present application relates to the technical field of robot trajectory planning, and in particular to a method and device for robot safe motion planning under homotopy-guided uncertain environments. Background Art

[0002] Safe motion planning, as an important research topic in the field of robotics, has been widely studied. Since the actual motion of onboard sensors and robots is not completely idealized, the safety of the task cannot be guaranteed by relying solely on deterministic conditions. Therefore, the robot needs to evaluate the impact of motion uncertainty and measurement noise on motion safety during motion. Robot navigation problems usually involve a planning module and a control module. Many traditional path planning methods rely on deterministic information, assuming that the motion is deterministic and the state is completely known, and leaving the uncertainty information to the control module. However, relying solely on the control module to process uncertainty information may lead to excessive computational burden and may even make the problem infeasible.

[0003] Motion planning methods that account for motion and observation uncertainty are often referred to as belief space planning (BSP), in which the robot's state is represented by a probability distribution over its states. By calculating the probability distribution of the robot at each state, the robot's collision probability in the current state can be effectively assessed, providing a decision-making basis for safe motion planning. BSP problems can often be formulated as partially observable Markov decision processes (POMDPs), but solving POMDPs is a very complex problem. Although recent advances in Markov solvers have greatly improved solution efficiency, solving POMDPs remains a challenging task.

[0004] In recent years, sampling-based motion planning algorithms, such as probabilistic road networks (PRMs) and rapidly exploring random trees (RRTs), have attracted widespread attention due to their efficient sampling in high-dimensional spaces and good scalability. Several studies have constructed motion planning frameworks based on sampling methods in uncertain environments. In motion planning with uncertainty, the chance-constrained programming (CCP) method is a classic solution that transforms the problem of safe obstacle avoidance into a form with probabilistic guarantees. Several research works have further developed the CCP method, enabling robotic systems to plan safely online and improving convergence to obtain more optimal solutions. However, these methods often lack consideration of actual observation information, resulting in overly conservative planning results. Therefore, it is crucial to simultaneously consider the uncertainty of both motion and observation.

[0005] To address this issue, a motion planning method combined with linear quadratic Gaussian (LQG) control has been proposed to evaluate feasible trajectories. This method first generates a large number of candidate trajectories using traditional RRT. Then, beliefs are propagated through LQG, and belief-based collision detection is performed to extract feasible and relatively optimal solutions from the large number of candidate trajectories. This method effectively handles decision-making under motion and observation uncertainty. However, due to the inherent stochasticity of RRT, this method requires generating a large number of candidate trajectories and selecting the optimal solution, resulting in low computational efficiency. To improve efficiency, methods based on belief graph structures have been proposed, such as the belief road network (BRM) and the rapidly exploring randomized belief tree (RRBT). BRM and RRBT use methods similar to those of PRM and rapidly exploring random graph (RRG), respectively, to construct a graph structure while propagating beliefs during the construction process. Ultimately, through multiple queries, feasible paths in the belief space are planned on the graph. Notably, these methods introduce interdependencies between adjacent nodes during the search process, leading to the "curse of history" problem. To address this issue, an improved BRM algorithm has been proposed. This method reduces dependencies by assigning fixed beliefs to each node and combining an LQG edge controller with a steady-state LQG (SLQG) controller to solve boundary value problems between adjacent belief nodes. Furthermore, a method based on covariance steering has been proposed to connect adjacent beliefs with fixed covariances to construct a BRM. These methods effectively avoid the dependency problem between adjacent nodes. However, these techniques require careful consideration of the reachability between nodes in the belief space and require solving a large number of two-point boundary value problems (TPBVPs) to construct the BRM, which is computationally expensive. Solving TPBVPs becomes more difficult as the system complexity increases. Furthermore, if the uncertainty parameters change, the graph structure must be rebuilt to support multiple query path searches, which is a major drawback of centralized sampling methods.

[0006] In recent years, incremental sampling methods for solving belief space planning problems have made steady progress. For example, an incremental sampling algorithm based on the RRT* framework has been proposed. This algorithm samples and expands the belief tree incrementally, using a rewiring mechanism to ensure asymptotic optimality. However, this rewiring process requires solving a large number of boundary value problems to optimize the belief nodes along the path.

[0007] Another significant challenge in belief space planning with kinematic constraints is the blindness of sampling. Since sampling is inherently random, a large number of samples are required to capture favorable states that yield near-optimal solutions. This problem is exacerbated when considering motion and observation uncertainty, as the set of valid sampled states is restricted to a narrow region, leading to an inefficient search process. Although existing methods have introduced reconnection mechanisms to shorten paths, this process remains computationally complex and time-consuming.

[0008] Therefore, how to efficiently plan a safe motion path for a mobile robot while taking into account motion and observation noise has become a technical problem that needs to be solved urgently in this field. Summary of the Invention

[0009] The purpose of this application is to provide a method and device for safe motion planning of robots in uncertain environments under homology guidance, which can enable mobile robots to efficiently search for optimal and belief-feasible paths in complex environments.

[0010] To achieve the above objectives, this application provides the following solutions:

[0011] In a first aspect, the present application provides a method for robot safe motion planning in an uncertain environment guided by homotopy, the method comprising:

[0012] A generalized Voronoi diagram (GVD) of the discretized space is obtained; the generalized Voronoi diagram includes a plurality of nodes that are equidistant from the nearest obstacle, and adjacent nodes are connected to each other.

[0013] Determine a spatial size attribute and a characteristic attribute of each node; the spatial size attribute corresponds to the radius of a maximum obstacle-free area centered on the node; and the characteristic attribute represents the number of nearest obstacles.

[0014] Based on the spatial size attribute and characteristic attribute of each node, the characteristic node is determined.

[0015] A feature graph is constructed based on the feature nodes.

[0016] Perform edge filtering on the feature map to obtain an updated feature map.

[0017] Performing a feature map search on the updated feature map to obtain a region of interest; the feature map search includes: a starting position and a target position of the task.

[0018] A non-uniform search is performed based on the region of interest to obtain an optimal path.

[0019] In a second aspect, the present application provides a computer device comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement any of the above-mentioned methods for safe motion planning of robots in uncertain environments guided by homology.

[0020] In a third aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements any of the above-mentioned methods for safe motion planning of robots in uncertain environments guided by homology.

[0021] In a fourth aspect, the present application provides a computer program product, comprising a computer program, which, when executed by a processor, implements any of the above-mentioned homotopy-guided safe motion planning methods for robots in uncertain environments.

[0022] According to the specific embodiments provided in this application, this application discloses the following technical effects:

[0023] This application provides a method and apparatus for safe robot motion planning in an uncertain environment guided by homology. By obtaining a generalized Voronoi diagram of a discretized space, which includes several nodes equidistant from the nearest obstacle and interconnected between adjacent nodes, the method can transform complex continuous space problems into discrete problems, facilitating computation and processing. By determining the spatial size attribute and characteristic attribute of each node, wherein the spatial size attribute corresponds to the radius of the maximum obstacle-free area centered on the node, and the characteristic attribute represents the number of nearest obstacles, the method can more comprehensively describe the environmental characteristics of each node and help distinguish the importance of different nodes. The spatial size attribute can help determine the feasibility and safety of the node, while the characteristic attribute can be used to assess the complexity and risk of the node. These attributes provide quantitative criteria for subsequent feature node selection, facilitating the selection of nodes that are more valuable for path planning. By determining feature nodes based on the spatial size attribute and characteristic attributes of each node and constructing a feature graph based on the feature nodes, the scope of path planning can be further narrowed, unnecessary computation can be reduced, and efficiency can be improved. At the same time, the feature graph retains the feature nodes and their connectivity, effectively reflecting the key features of path planning. By performing edge filtering on the feature graph, an updated feature graph is obtained; unimportant edges or nodes can be removed, further reducing the scope of the path search and improving the search efficiency. By performing a feature graph search on the updated feature graph, a region of interest is obtained; the feature graph search includes: the starting position and target position of the task; the region of interest containing the starting point and the target point can be quickly located, further reducing the scope of the path search, avoiding unnecessary searches in the global feature graph, and improving the search efficiency. By performing a non-uniform search based on the region of interest, the optimal path is obtained; the spatial size attributes and feature attributes of the nodes can be better utilized to plan a safer and more efficient path; the non-uniform search can dynamically adjust the search strategy according to the complexity of the environment, and is suitable for path planning in various complex environments. Compared with the existing BSP method, the present application has a homotopy-guided non-uniform sampling strategy and avoids the need to solve the boundary value problem. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0025] Figure 1 This is a diagram of the application environment of a homotopy-guided robot safe motion planning method in an uncertain environment in one embodiment of the present application.

[0026] Figure 2 A flowchart of a method for safe robot motion planning in an uncertain environment guided by homotopy is provided in one embodiment of the present application.

[0027] Figure 3 A schematic diagram of state space feature map extraction provided in one embodiment of the present application.

[0028] Figure 4 A schematic diagram of filtering branches where the belief space is infeasible according to an embodiment of the present application.

[0029] Figure 5 A schematic diagram of determining a homotopy sampling space provided in an embodiment of the present application.

[0030] Figure 6 A schematic diagram of the process of determining homology classes in a belief space provided in one embodiment of the present application.

[0031] Figure 7 A schematic diagram of a non-uniform search process provided in one embodiment of the present application.

[0032] Figure 8 A schematic diagram of the structure of a computer device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0033] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0034] The present application aims to efficiently plan a safe motion path for a mobile robot while taking into account motion and observation noise. Specifically, the present application aims to enable a mobile robot to efficiently search for an optimal, belief-feasible path in a complex environment. The method first identifies the feasible topology of the space and uses steady-state belief estimation to determine the optimal feasible homology class, thereby achieving non-uniform search. In addition, the method adopts an incremental state expansion strategy to avoid the computationally expensive solution of the two-point boundary value problem. After reaching the target area, a steady-state linear quadratic Gaussian (SLQG) controller is used to ensure a smooth transition to steady state.

[0035] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0036] The robot safe motion planning method under uncertain environment provided by the embodiment of the present application can be applied to Figure 1 In the application environment shown, the terminal 102 communicates with the server 104 via a network. The data storage system can store data that the server 104 needs to process. The data storage system can be set up separately, integrated on the server 104, or placed on the cloud or other servers. Terminal 102 may send a generalized Voronoi diagram of the discretized space to server 104. The generalized Voronoi diagram includes a plurality of nodes equidistant from the nearest obstacle, with adjacent nodes interconnected. After receiving the generalized Voronoi diagram of the discretized space, server 104 may determine a spatial size attribute and a characteristic attribute of each node in the discretized space. The spatial size attribute corresponds to the radius of the maximum obstacle-free area centered on the node. The characteristic attribute represents the number of nearest obstacles. Based on the spatial size attribute and characteristic attribute of each node, a characteristic node may be determined. A feature graph may be constructed based on the characteristic nodes. Edge filtering may be performed on the feature graph to obtain an updated feature graph. A feature graph search may be performed on the updated feature graph to obtain a region of interest. The feature graph search may include determining the starting and target locations of the task. A non-uniform search may be performed based on the region of interest to obtain an optimal path. Server 104 may provide feedback on the obtained optimal path to terminal 102. In addition, in some embodiments, the method for safe motion planning of a robot under an uncertain environment guided by homology can also be implemented separately by the server 104 or the terminal 102. For example, the terminal 102 can directly perform safe motion planning of a robot under an uncertain environment guided by homology for the generalized Voronoi diagram of the discretized space, or the server 104 can obtain the generalized Voronoi diagram of the discretized space from the data storage system and perform safe motion planning of a robot under an uncertain environment guided by homology for the generalized Voronoi diagram of the discretized space.

[0037] The terminal 102 may be, but is not limited to, various desktop computers, laptop computers, smart phones, and tablet computers. The server 104 may be implemented as an independent server or a server cluster consisting of multiple servers, or a cloud server.

[0038] In an exemplary embodiment, Figure 2 As shown, a method for safe motion planning of a robot under an uncertain environment guided by homology is provided. The method is executed by a computer device, specifically a computer device such as a terminal or a server, or a terminal and a server. In the embodiment of the present application, the method is applied to Figure 1 The server 104 in the example is used as an example to illustrate the method, which includes the following steps S1 to S7.

[0039] S1: Obtain a generalized Voronoi diagram of the discretized space; the generalized Voronoi diagram includes a plurality of nodes that are equidistant from the nearest obstacle, and adjacent nodes are connected to each other.

[0040] S2: Determine the spatial size attribute and characteristic attribute of each node; the spatial size attribute corresponds to the radius of the maximum obstacle-free area centered on the node; the characteristic attribute represents the number of nearest obstacles.

[0041] S3: Determine the characteristic nodes based on the spatial size attribute and characteristic attribute of each node.

[0042] S4: Construct a feature graph based on the feature nodes.

[0043] S5: Perform edge filtering on the feature map to obtain an updated feature map.

[0044] S6: Performing a feature map search on the updated feature map to obtain a region of interest; the feature map search includes: a starting position and a target position of the task.

[0045] S7: Perform a non-uniform search based on the region of interest to obtain an optimal path.

[0046] By implementing the above steps S1 to S7, the present application proposes a hierarchical homology-driven belief space planning framework for solving path planning problems under motion and observation uncertainty. The framework can effectively deal with uncertainties in complex environments and improve the reliability and efficiency of path planning. At the same time, the present application introduces a method for constructing belief feasible homology classes, which can effectively identify feasible homology classes in the belief space and use these homology classes to guide the path planner to perform efficient belief space planning, thereby reducing the exploration of invalid areas and improving computational efficiency. In addition, the present application designs an incremental non-uniform sampling strategy. This strategy performs non-uniform sampling in the belief space, avoiding the need to solve complex time boundary value problems, and by limiting the search range to lower-cost homology classes, quickly converges to a trajectory close to the optimal solution, significantly improving the quality and computational efficiency of the solution. The HDIBT planning method for safe motion planning in belief space proposed in the present application. Compared with the existing BSP method, it has a homology-guided non-uniform sampling strategy and avoids the need to solve boundary value problems.

[0047] As an optional implementation, in step S5, edge filtering is performed on the feature map to obtain an updated feature map, specifically including:

[0048] S51: Extract features from the feature map to obtain spatial features in the state space.

[0049] S52: Based on the spatial features, an iterative method is used to calculate a steady-state covariance matrix.

[0050] S53: Compare the ellipse corresponding to the steady-state covariance matrix with the spatial volume of each edge to determine whether the steady-state covariance matrix is ​​valid in the belief space, and obtain a determination result.

[0051] S54: If the judgment result is no, the invalid edge will be removed.

[0052] S55: If the judgment result is yes, the corresponding edge is retained to obtain an updated feature map.

[0053] As an optional implementation, in step S6, performing a feature map search on the updated feature map to obtain a region of interest specifically includes:

[0054] S61: Map the starting position and the target position to the updated feature map to obtain a mapped feature map.

[0055] S62: Perform a topological search on the mapped feature graph to identify the shortest homotopic topological path on the mapped feature graph.

[0056] S63: Map the shortest homotopic topological path to free space to obtain a region of interest.

[0057] As an optional implementation, in step S7, a non-uniform search is performed based on the region of interest to obtain an optimal path, specifically including:

[0058] S71: Perform random sampling in the region of interest to obtain random sampling nodes.

[0059] S72: Performing a nearest neighbor search on the belief tree to determine the node closest to the randomly sampled node and using it as the parent node; the belief tree is a tree structure obtained by stepwise sampling in the belief space.

[0060] S73: Perform forward integration on the belief tree along the direction from the parent node to the randomly sampled node to expand new nodes and edges.

[0061] S74: Perform belief propagation on the new nodes and edges, and use a Monte Carlo sampling method to determine the collision probability between the edge belief and the obstacle.

[0062] Specifically, a collision detection function is used to determine the collision ratio between edge beliefs and obstacles.

[0063] A collision probability between the edge belief and the obstacle is determined based on the collision ratio.

[0064] S75: Counting the proportion of collisions occurring in all sampling points, and when the proportion of collisions occurring in all sampling points is less than a set tolerance collision probability threshold, updating the belief tree to obtain an updated belief tree.

[0065] S76: When the updated belief tree is extended close to the target location, an optimal path is obtained.

[0066] Specifically, in order to improve the efficiency of the belief tree search algorithm and avoid exploring irrelevant free space, a key method is to identify the region of interest so as to quickly expand the sampling. Therefore, this application designs a method to identify different homology classes and determine the feasible sampling space based on the belief covariance. The specific extraction process is as follows Figure 3-Figure 5 As shown, it includes the following steps: feature map extraction ( Figure 3 ), edge filtering( Figure 4 ) and homotopy search ( Figure 5 ).

[0067] The purpose of constructing feature maps is to concisely represent the structural characteristics of the space, especially the region division caused by arbitrarily distributed obstacles.

[0068] Step 1: Compute the generalized Voronoi diagram of the discretized space. The GVD consists of multiple nodes (i.e., GVD nodes) that are equidistant from the nearest obstacle, and adjacent GVD nodes are interconnected. Therefore, the GVD captures the connectivity of the entire space and delineates the partitions caused by obstacles. However, a large number of GVD nodes can impose a significant computational burden.

[0069] To solve this problem, key GVD nodes are extracted as feature nodes to simplify the graph representation. Specifically, representative nodes are selected from the GVD node set as feature nodes to simplify the graph structure. GVD nodes are nodes with at least two nearest obstacles in the free space. Therefore, once the GVD node set V is determined, G , we can determine the spatial size attribute and characteristic attribute of each node. The spatial size attribute corresponds to the radius of the maximum obstacle-free area centered on the node, while the characteristic attribute represents the number of the nearest obstacles. Figure 3 As can be seen from the circular areas in , the number of closest obstacles to most of these nodes is 2 (as determined by the number of contact points between these circles and obstacles), while a few nodes have more than 2. Furthermore, nodes with 3 closest obstacles are connected to nodes with 2 closest obstacles, similar to the relationship between skeleton joints and skeletons. For each point in the initially extracted GVD node set, the number of closest obstacles is calculated, and nodes with more than 2 closest obstacles are selected as feature nodes.

[0070] After extracting the feature nodes, their connectivity is established by verifying whether the two feature nodes are only connected through the GVD node with the nearest obstacle number of 2. The final feature graph is constructed as follows Figure 3 The yellow nodes and green dashed lines in the figure are shown, where the purple circular area represents the maximum obstacle-free area centered on the GVD node, thus reflecting the spatial extent of the respective areas.

[0071] Step 2: The feature map extraction process identifies spatial features in the state space. However, in the belief space, the state is represented by a probability distribution, so additional processing is required to derive a feasible state map. Due to the initial uncertainty and the presence of motion-observation noise, the belief covariance stabilizes over time after Kalman filtering. The feature map update process uses this steady-state belief to refine the feature map by filtering out infeasible areas. First, the steady-state belief is calculated using an iterative method to determine the steady-state covariance. Subsequently, the steady-state belief Σ s The spatial volume of each edge is compared to evaluate its validity in the belief space. Invalid edges are removed to update the feature map. The final updated feature map structure is shown in the attached Figure 4 shown.

[0072] Specifically, feature extraction is performed on the feature graph obtained in step 1 above to identify the spatial features in the state space, and an iterative method is used to calculate the steady-state belief based on the spatial features to determine the steady-state covariance matrix. Specifically, in a discrete-time linear system, consider the following state and observation equations:

[0073]

[0074] Among them, x k+1 is the state equation; z k is the observation equation; x k is the current state; u k is the control input; is the system noise, W is the system noise matrix; is the observation noise, V is the observation noise matrix; A is the system matrix; B is the control matrix; C is the observation matrix.

[0075] The Kalman filter first makes a prediction at each step:

[0076]

[0077] in, is the state estimate at the current moment; is the state estimate at time k-1; u k-1 is the control input at time k-1; P k|k-1 is the state error estimate at the current moment; P k-1|k-1 is the state error estimate at time k-1.

[0078] Then update:

[0079]

[0080] Among them, K k is the Kalman gain; is the updated state estimate; P k|k is the updated state error estimate; I is the identity matrix.

[0081] In order to obtain the steady-state covariance matrix P ∞ , we need to solve the following discrete algebraic Riccati equation (DARE):

[0082] P ∞ =AP ∞ A T +W-AP ∞ C T (CP ∞ C T +V) -1 CP∞ A T ;

[0083] The equation can be approximately solved by iterative method: take the initial value P (0) .

[0084] P (i+1) =AP (i) A T +W-AP (i) C T (CP (i) C T +V) -1 CP (i) A T ;

[0085] Recursively, until the covariance belief region is stable, that is, ‖P (i+1) -P (i) ‖<ε′, ε′ means the covariance belief region is stable, then let Σ s =P (i+1) . Subsequently, the steady-state belief Σ s The corresponding ellipse is compared with the spatial volume of each edge to evaluate its validity in the belief space. Invalid edges are removed to update the feature map. The final updated feature map structure is shown in the attached Figure 4 shown.

[0086] Step 3: Based on the final feature graph structure obtained in step 2, perform feature graph search to extract the region of interest that is beneficial to the belief tree search. This search includes the starting position and target position of the task. The belief space homology class determination process is as follows: Figure 6 shown.

[0087] Initially, the starting point and the target point are mapped to the feature graph structure of the belief feasibility. Then, the algorithm is used to identify The shortest homotopic topological path on the feature graph is G, where G is the node of the feature graph and E is the edge of the feature graph. The search process is as follows:

[0088] First, the graph Initialize the weight function c(u,n): For each node n∈V, set the shortest distance estimate d(n) = ∞, and the predecessor pointer π(n) = null, set the distance from the starting point to d(s) = 0, and add all nodes to the minimum priority queue Q. Then, repeat the following steps: pop the node with the minimum value from Q (satisfying d(u) = min x∈Qd(x)), check each outgoing edge (u→n)∈E, if d(u)+c(u,n)<d(n), then update d(n)←d(u)+c(u,n),π(n)←u and adjust the priority in the queue; where d represents the function that measures the node cost; c represents the cost between two nodes. After the loop ends, d(n) is the shortest distance, and π can be traced back along the predecessor pointer to obtain the shortest path. After completing the topology search, the identified topology Γ is obtained through Map to free space to obtain the region of interest Ω (i.e., homotopy sampling space):

[0089]

[0090] Where, Ω is the region of interest; Represents mapping the topological path to equidistant points, V G is a node set; represents the maximum circular area of ​​each equidistant point; r(v) represents the minimum obstacle distance of node v.

[0091] The HDIBT algorithm gradually samples in the belief space to build a belief tree in, is the node of the tree; ε is the edge of the tree. The uncertainty in the system dynamics as well as the motion and observation is taken into account. By performing a non-uniform search within the region of interest, the HDIBT algorithm reduces the risk of getting stuck in a high-cost local solution during the global search. In addition, during the expansion process, each expansion of the tree advances the system model to generate a nominal trajectory segment, thus avoiding the computational burden of solving the time-consuming boundary value problem. The detailed search is shown in Figure 7 The corresponding detailed description is as follows:

[0092] Input: The input of the HDIBT algorithm includes the configuration space Initial belief mean Initial covariance Σ0, target mean and the maximum number of iterations iter max In addition, the algorithm also requires a region of interest Ω for sampling, which is determined in the previous section.

[0093] Initialization: Before each search begins, the tree structure parameters must be initialized, including the settings of the root node and edges. Each node contains five attributes: belief node mean nx, node covariance nP, node cost n.cost, parent node n.parent, and control steering input n.control.

[0094] Sampling: The HDIBT algorithm restricts the sampling area to the identified region of interest Ω. In each search iteration, the algorithm performs random sampling within Ω to effectively guide the expansion of the belief tree.

[0095] Extension: The HDIBT algorithm extends the tree structure by forward integration. Then, the function performs a nearest neighbor search to find the nearest randomly sampled node x in the tree. rand At this stage, the HDIBT algorithm (which is different from the traditional RRT algorithm. The standard RRT usually uses the two-point boundary value method to directly connect the two states. However, this method is computationally challenging for high-order systems. To overcome this problem, the algorithm does not directly connect the two states) moves along the path from the nearest randomly sampled node x nearest To randomly sample node x rand In the direction of the parent node, forward integration is performed. This process eventually generates a new state x new and its corresponding trajectory e new .

[0096] Check and Update: After each belief trajectory is extended, the belief is first propagated and updated to obtain the belief associated with the extended edge. Subsequently, the collision probability between the edge belief and the obstacle is evaluated. During the obstacle avoidance process, a Monte Carlo sampling-based method is used. Specifically, for the current belief state, a corresponding covariance ellipse is constructed, and a set of state points are randomly sampled within the ellipse. For each sampling point x i Detect whether it collides with an obstacle. The collision detection function is:

[0097]

[0098] Count the proportion of collisions among all sampling points ρ, if ρ<ρ th , where ρ th If the collision probability threshold is set, the current belief state is considered feasible; otherwise, it is considered infeasible. The algorithm effectively handles collision detection in complex environments. Only when the check successfully passes will the node and edge be included in the tree structure.

[0099] Goal Check: When the belief tree is expanded close to the goal region, the final motion plan in the belief space is returned.

[0100] The above process repeats itself each time the search plan is executed until the termination criteria are met. This method remains computationally efficient by avoiding boundary value problems for the kinematic system. Multiple search iterations are performed to ensure convergence and the optimal path, ultimately returning the optimal solution.

[0101] This application provides a homotopy-guided method for safe robot motion planning in an uncertain environment. Compared with the existing technology, it has the following technical effects:

[0102] 1. Hierarchical Homotopy-Driven Belief Space Planning Framework: This application proposes a hierarchical homotopy-driven belief space planning framework for solving path planning problems under motion and observation uncertainty. This framework effectively addresses uncertainty in complex environments and improves the reliability and efficiency of path planning.

[0103] 2. Method for Constructing Feasible Belief Homotopy Classes: This application introduces a method for constructing feasible belief homotopy classes. This method can effectively identify feasible homotopy classes in the belief space and use these homotopy classes to guide the path planner to perform efficient belief space planning, thereby reducing the exploration of invalid regions and improving computational efficiency.

[0104] 3. Incremental Non-Uniform Sampling Strategy: This application designs an incremental non-uniform sampling strategy. This strategy performs non-uniform sampling in the belief space, avoiding the need to solve complex time boundary value problems. By limiting the search scope to low-cost homotopy classes, it quickly converges to a trajectory close to the optimal solution, significantly improving the solution quality and computational efficiency.

[0105] The present application also provides an application scenario, which applies the above-mentioned homotopy-guided robot safe motion planning method in an uncertain environment. Specifically: the homotopy-guided robot safe motion planning method in an uncertain environment provided by this embodiment can be applied in a robot trajectory planning scenario. The robot trajectory planning scenario includes: a generalized Voronoi diagram acquisition link, an attribute determination link, a feature node determination link, a feature graph construction link, an update link, a feature graph search link and a non-uniform search link; first, a generalized Voronoi diagram of a discretized space is obtained; the generalized Voronoi diagram includes several nodes that are equidistant from the nearest obstacles, and adjacent nodes are interconnected; second, the spatial size attribute and feature attribute of each node are determined; the spatial size attribute corresponds to the radius of the maximum obstacle-free area centered on the node; the feature attribute represents the number of nearest obstacles; based on the spatial size attribute and feature attribute of each node, a feature node is determined; a feature graph is constructed based on the feature nodes; then, the feature graph is edge filtered to obtain an updated feature graph; a feature graph search is performed on the updated feature graph to obtain a region of interest; the feature graph search includes: the starting position and target position of the task; finally, a non-uniform search is performed based on the region of interest to obtain the optimal path.

[0106] In an exemplary embodiment, a computer device is provided. The computer device may be a server or a terminal. The internal structure diagram thereof may be as follows: Figure 8As shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, abbreviated as I / O) and a communication interface. The processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store the generalized Voronoi diagram of the discretized space. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a method for safe motion planning of a robot in an uncertain environment guided by homology is implemented.

[0107] Those skilled in the art will understand that Figure 8 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than shown in the figure, or combine certain components, or have a different component arrangement.

[0108] In an exemplary embodiment, a computer device is further provided, including a memory and a processor. The memory stores a computer program, and the processor implements the above method embodiments when executing the computer program.

[0109] In an exemplary embodiment, a computer-readable storage medium is provided, storing a computer program, which implements the above-mentioned method embodiments when executed by a processor.

[0110] In an exemplary embodiment, a computer program product is provided, including a computer program. When the computer program is executed by a processor, the above method embodiments are implemented.

[0111] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0112] Those skilled in the art will understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, database or other media used in the embodiments provided in this application may include at least one of non-volatile and volatile memory. Non-volatile memory may include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory may include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0113] The databases involved in the various embodiments provided herein may include at least one of a relational database and a non-relational database. Non-relational databases may include, but are not limited to, distributed databases based on blockchains. The processors involved in the various embodiments provided herein may include, but are not limited to, general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic units, data processing logic units based on quantum computing, and the like.

[0114] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0115] This document uses specific examples to illustrate the principles and implementation methods of this application. The description of the above examples is only intended to help understand the method and core concept of this application. At the same time, for those skilled in the art, based on the concept of this application, there may be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A homotopy-guided robot safe motion planning method in an uncertain environment, characterized by: The method for safe robot motion planning under an uncertain environment guided by homotopy includes: Obtaining a generalized Voronoi diagram of the discretized space; the generalized Voronoi diagram includes a plurality of nodes equidistant from the nearest obstacle, and adjacent nodes are connected to each other; Determining a spatial size attribute and a characteristic attribute of each node; the spatial size attribute corresponds to the radius of a maximum obstacle-free area centered on the node; and the characteristic attribute represents the number of nearest obstacles; Determine the characteristic nodes based on the spatial size attribute and characteristic attribute of each node; Constructing a feature graph based on the feature nodes; Performing edge filtering on the feature map to obtain an updated feature map; Performing a feature map search on the updated feature map to obtain a region of interest; the feature map search includes: a starting position and a target position of the task; A non-uniform search is performed based on the region of interest to obtain an optimal path.

2. The method for safe robot motion planning in an uncertain environment guided by homotopy according to claim 1 is characterized in that: Performing edge filtering on the feature map to obtain an updated feature map specifically includes: Performing feature extraction on the feature map to obtain spatial features in the state space; Based on the spatial characteristics, an iterative method is used to calculate the steady-state covariance matrix; Comparing the ellipse corresponding to the steady-state covariance matrix with the spatial volume of each edge, determining whether the steady-state covariance matrix is ​​valid in the belief space, and obtaining a determination result; If the judgment result is negative, the invalid edge will be removed; If the judgment result is yes, the corresponding edge is retained to obtain an updated feature map.

3. The method for safe robot motion planning under uncertain environment guided by homotopy according to claim 2 is characterized in that: The calculation formula of the steady-state covariance matrix is: P ∞ =AP ∞ A T +W-AP ∞ C T (CP ∞ C T +V) -1 CP ∞ A T ; Among them, P ∞ is the steady-state covariance matrix; A is the system matrix; W is the system noise matrix; C is the observation matrix; V is the observation noise matrix.

4. The method for safe robot motion planning under uncertain environment guided by homotopy according to claim 1, characterized in that: Performing a feature map search on the updated feature map to obtain a region of interest specifically includes: Mapping the starting position and the target position to the updated feature map to obtain a mapped feature map; Performing a topological search on the mapped feature graph to identify the shortest homotopic topological path on the mapped feature graph; The shortest homotopic topological path is mapped to free space to obtain a region of interest.

5. The method for safe robot motion planning in an uncertain environment guided by homotopy according to claim 1, characterized in that: The expression of the region of interest is: Where, Ω is the region of interest; Represents mapping the topological path to equidistant points, V G is a node set; represents the maximum circular area of ​​each equidistant point; r(v) represents the minimum obstacle distance of node v.

6. The method for safe robot motion planning in an uncertain environment guided by homotopy according to claim 1, characterized in that: Performing a non-uniform search based on the region of interest to obtain an optimal path specifically includes: Perform random sampling in the region of interest to obtain random sampling nodes; Performing a nearest neighbor search on the belief tree to determine the node closest to the randomly sampled node and using it as the parent node; the belief tree is a tree structure obtained by stepwise sampling in the belief space; Performing forward integration on the belief tree along the direction from the parent node to the randomly sampled node to expand new nodes and edges; Performing belief propagation on the new nodes and edges, and using a Monte Carlo sampling method to determine the collision probability between the edge belief and the obstacle; Count the proportion of collisions among all sampling points, and when the proportion of collisions among all sampling points is less than the set tolerance collision probability threshold, update the belief tree to obtain an updated belief tree; When the updated belief tree is expanded to be close to the target location, the optimal path is obtained.

7. The method for safe robot motion planning under uncertain environment guided by homotopy according to claim 6, characterized in that: Belief propagation is performed on the new nodes and edges, and the collision probability between the edge beliefs and obstacles is determined using a Monte Carlo sampling method, specifically including: A collision detection function is used to determine the collision ratio between edge beliefs and obstacles; determining a collision probability between the edge belief and the obstacle based on the collision ratio; The expression of the collision detection function is: Among them, collision(x i ) is the collision detection function; x i is the i-th sampling point.

8. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the homotopy-guided robot safe motion planning method in an uncertain environment according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for safe motion planning of a robot under an uncertain environment guided by homotopy is realized according to any one of claims 1 to 7.

10. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the method for safe motion planning of a robot under an uncertain environment guided by homotopy is realized according to any one of claims 1 to 7.

Images