A homotopy perception safety corridor generation method for robot motion planning

Abstract

This invention discloses a homotopy-aware safe corridor generation method for robot motion planning, belonging to the field of robot autonomous navigation and motion planning technology. First, an adaptive multi-scale convex graph representation of the robot's free space is constructed. Candidate safe corridors connecting the start and end points of the motion path are searched on the convex graph, and an execution trajectory is selected. During online robot execution, local environmental perception information is acquired in real time, and the convex graph is locally updated. When the current trajectory fails, a local safe corridor search is first performed on the updated convex graph, and a consistent visible deformation criterion is defined to obtain a fused topologically redundant corridor. When local repair fails, the system reverts to the convex graph to perform global replanning, thus achieving the generation, maintenance, and updating of safe corridors with different topologies without relying on predefined reference paths. This invention can improve the efficiency and robustness of safe corridor generation and trajectory planning for robot motion in complex environments.

Description

Technical Field

[0001] This invention belongs to the field of robot autonomous navigation and motion planning technology, and relates to technologies related to robot safety corridor construction, free space representation, topological constraint planning and collision avoidance. Specifically, it relates to a homotopy-aware safety corridor generation method for robot motion planning, which is used to generate homotopy-aware safety corridors for robot collision-free motion planning in complex obstacle environments without the need for predefined reference paths. Background Technology

[0002] In robot motion planning, to ensure collision-free passage through complex obstacle environments and improve the stability of subsequent trajectory optimization, a safe corridor consisting of several feasible convex regions is typically constructed in free space first. Trajectory generation and optimization then take place within this safe corridor. The safe corridor provides the robot with a feasible motion area that satisfies obstacle avoidance constraints, and therefore has become a commonly used intermediate representation in trajectory planning in complex environments.

[0003] In existing technologies, a common method for generating safe corridors is based on constructing predefined reference paths. Specifically, a discrete reference path is typically obtained first using a search or sampling algorithm, and then a series of interconnected convex regions are generated segment by segment along this reference path to form a safe corridor. In this type of method, the geometry and topology of the safe corridor are largely inherited from the reference path, making the implementation relatively straightforward and applicable in many scenarios. However, the aforementioned corridor generation methods based on reference paths still have significant shortcomings. First, the reference path itself is a low-dimensional geometric prior, while the safe corridor is a high-dimensional feasible region representation. This mismatch in representation makes the generated corridor highly sensitive to local perturbations of the reference path, resulting in elongated, irregular, or unstable corridor structures that affect subsequent trajectory optimization. Second, since the corridor generation process relies on a single predefined reference path, the topological categories of the paths that can be explored are often implicitly limited by this reference path. In environments with multiple passable topological channels, it is difficult to systematically discover and maintain multiple topologically distinct candidate corridors.

[0004] To improve the representation capability of free space, the Graphs of ConvexSets (GCS) method has been proposed in existing technologies. This method typically represents free space as a graph structure consisting of multiple convex regions and their connections, where graph nodes correspond to convex regions and graph edges correspond to non-empty overlapping relationships between convex regions. This allows for the combination of graph search and convex optimization for path planning and trajectory optimization. GCS methods can compactly describe complex free spaces and support planning on region sequences. However, most existing GCS methods rely on global static convex decomposition, typically assuming that the environment decomposition remains unchanged during the planning process. Therefore, they often struggle to adapt efficiently when environmental information changes or local regions fail. Furthermore, existing GCS methods primarily focus on free space representation and region sequence optimization; for the safety corridors themselves composed of convex set sequences, they lack topological similarity determination and redundancy resolution mechanisms at the corridor level, easily leading to the problem of corridors corresponding to different search results being topologically duplicated.

[0005] Furthermore, while existing homotopy-aware motion planning methods can generate feasible paths with different topologies, they mostly define and determine homotopy categories at the "path" level, such as based on path continuity deformation relationships or visibility constraints. However, in practical robot planning, dynamic feasibility constraints, collision avoidance constraints, and subsequent trajectory optimization are usually applied at the corridor or region level, rather than directly on a single path. Therefore, existing path-level homotopy analysis methods are difficult to directly apply to the generation and maintenance of safe corridors, resulting in a mismatch between path-level topology discrimination and corridor-level optimization objects.

[0006] Therefore, existing technologies still lack a method for generating safe corridors for robot motion planning. Existing robot safe corridor generation technologies cannot meet the needs of both eliminating dependence on predefined reference paths and constructing safe corridors directly on free space representations, and also distinguishing candidate solutions with different topologies at the corridor level, eliminating topological redundancy, and supporting efficient updates when there are uncertainties or local changes in the environment. The efficiency and robustness of safe corridor generation and trajectory planning in complex environments are not high. Summary of the Invention

[0007] To overcome the shortcomings of the prior art, this invention provides a homotopy-aware safe corridor generation method for robot motion planning, which solves the problems of existing safe corridor generation methods relying on predefined reference paths, difficulty in performing topology discrimination at the corridor level, and low efficiency of local updates.

[0008] In this invention, homotopy perception is embodied in visibility deformation (VD) and uniform visibility deformation (UVD). In the context of paths, visibility deformation and uniform visibility deformation are expressions of homotopy, subsets of path-to-path homotopy. This invention extends visibility deformation and uniform visibility deformation from the path level to the level of safe corridors (i.e., convex set sequences), termed homotopy perception.

[0009] The core of this invention lies in its adoption of a safe corridor generation paradigm that eliminates the need for predefined reference paths. It directly constructs a convex set graph and searches for safe corridors directly on this graph, thus avoiding the geometric biases and topological constraints imposed by reference paths. An adaptive multi-scale convex set graph (coarse-scale + fine-scale) is constructed, enabling incremental updates without overall reconstruction when the environment changes locally. Direct searching on this convex set graph natively supports safe corridor generation, eliminating the need for expansion and regeneration of safe corridors from reference paths, including both offline construction and online updates. Simultaneously, a consistent visible deformation criterion is designed for the safe corridor (represented as a convex set sequence) hierarchy to detect topological redundancy between safe corridors and to fuse topologically redundant corridors. This invention not only eliminates the dependence on predefined reference paths, constructing safe corridors directly on free space representations, but also distinguishes candidate solutions with different topologies at the corridor level, eliminates topological redundancy, and supports efficient updates when the environment is uncertain or undergoes local changes. This improves the efficiency and robustness of safe corridor generation and trajectory planning in complex environments.

[0010] This invention proposes a homotopy-aware safe corridor generation method for robot motion planning. Under conditions of complex environments and local map uncertainty, it directly generates and maintains safe corridors connecting the start and end points of the robot's planned motion route based on free space representation. This method uses a convex set graph as a structured representation of free space, representing a safe corridor as a sequence of adjacent convex sets. By constructing an adaptive multi-scale convex set graph, it achieves coordinated global topology exploration and local geometric updates. Unlike corridor generation methods that rely on predefined reference paths, this invention does not pre-generate discrete reference paths but directly searches for candidate safe corridors on the convex set graph. Simultaneously, it extends visibility deformation (VD) and uniform visibility deformation (UVD) from the path level to the convex set sequence level to determine the topological similarity between different safe corridors, fuse topologically redundant corridors, and retain candidate corridors with different topologies. When local obstacle updates occur in the environment, this invention only performs local updates and local replanning on the affected area, without requiring global reconstruction of the entire graph.

[0011] The technical solution provided by this invention is as follows:

[0012] To address the collision-free motion planning problem for robots in complex environments, an adaptive multi-scale convex graph representation of free space is first constructed based on a prior map. Then, candidate safe corridors connecting the start and end points of the motion path are directly searched on the coarse-scale convex graph, and the execution trajectory is selected from multiple candidate corridors based on trajectory evaluation cost. During the robot's online execution, local environmental perception information is acquired in real time, and the multi-scale convex graph is locally updated. When the current trajectory fails, a local safe corridor search is first performed on the updated fine-scale convex graph, and a fused topological redundancy corridor is obtained using the consistent visibility deformation criterion. When local repair fails, the robot reverts to the coarse-scale convex graph to perform global replanning, thereby achieving the generation, maintenance, and updating of different topological safe corridors without relying on predefined reference paths.

[0013] The homotopy-aware safe corridor generation method for robot motion planning provided by this invention includes the following steps:

[0014] 1) Establish the configuration space model, obstacle set, and free space set for robot motion planning:

[0015] A configuration space is established based on the robot's motion dimensions, and a set of convex obstacles is obtained from the environment map. When non-convex obstacles exist in the environment, they are decomposed or approximated as multiple convex obstacles. Furthermore, the free space set is defined as the feasible region in the configuration space after removing obstacles, and the robot's start and end points are given.

[0016] 2) Construct a fine-scale convex set graph within the free space set:

[0017] Collision-free sampling is performed within the free space set, and each sampling point is extended to the surrounding collision-free region as a fine-scale convex set. ,in This represents a fine-scale convex set, where the upper bound of the scale of each fine-scale convex set is determined by a preset resolution. Constraints are imposed; then, using fine-scale convex sets as graph nodes, and with the condition that two fine-scale convex sets have a non-empty overlap as the connection condition, a fine-scale convex set graph is constructed. , This represents the set of nodes in a fine-scale convex graph structure. This represents the set of edge relations in a fine-scale convex graph structure. Fine-scale convex graphs are used to finely describe the local geometry of free space and provide a basis for subsequent local updates.

[0018] 3) Construct a coarse-scale convex set based on the fine-scale convex set, and form an adaptive multi-scale convex set:

[0019] From fine-scale convex set diagrams We select root nodes that are not visible to each other, and use these root nodes as starting points to traverse and aggregate on the fine-scale graph. We then merge the fine-scale convex sets that belong to the same aggregation result and take their convex hulls. If the convex hulls satisfy the collision-free safety condition, we obtain the corresponding coarse-scale convex set. Then, using coarse-scale convex sets as graph nodes and non-empty overlaps between coarse-scale convex sets as the condition for connecting edges, a coarse-scale convex set graph is constructed. , This represents the set of nodes in a coarse-scale convex graph structure. This represents the set of edge relations in a coarse-scale convex graph structure. An adaptive multi-scale convex graph is formed by combining fine-scale and coarse-scale convex graphs. Among them, fine-scale convex sets are used for local geometry preservation and local updates, while coarse-scale convex sets are used for global connectivity representation and corridor search.

[0020] 4) Connect the start and end points of the robot's motion planning route to a multi-scale convex graph, and search for candidate safe corridors on the coarse-scale convex graph:

[0021] The start and end points are connected to the multi-scale convex set nodes containing them, denoted as the start node and end node, respectively. On the coarse-scale convex set graph, one or more candidate convex set sequences are searched from the start node to the end node, resulting in a set of candidate safe corridors. Each candidate safe corridor is represented as an ordered sequence of convex sets, corresponding to a feasible corridor from the start to the end point. Because the coarse-scale convex set graph is constructed using a visibility-based aggregation method, this step can efficiently discover topologically distinct candidate corridors without relying on predefined reference paths.

[0022] 5) Generate candidate trajectories based on candidate safety corridors, and select the current execution trajectory:

[0023] For one or more candidate safety corridors obtained in step 4), trajectory optimization is performed within each candidate corridor to obtain a corresponding candidate trajectory set; then, based on a preset trajectory evaluation cost function, the trajectory with the optimal cost is selected from the candidate trajectory set as the current execution trajectory. The trajectory evaluation cost function can be set to trajectory duration, trajectory length, average speed, smoothness, or a combination thereof, depending on application needs.

[0024] In practical implementation, this invention selects several candidate safety corridors, solves the trajectory based on the safety corridors, and evaluates the obtained trajectories according to user preferences (such as short trajectory length, short trajectory time, smoother trajectory, etc.) for one or more of the preferences, and selects one of them as the execution trajectory. The trajectory evaluation cost is an evaluation function composed of one or more of the characteristic costs such as trajectory length, total trajectory time, and trajectory smoothness.

[0025] 6) During robot execution, receive local environmental perception information and identify affected fine-scale convex sets:

[0026] In practice, local environmental perception information refers to the new local environmental features acquired by the robot in real time through perception. Further information is then extracted from this information, including: newly added obstacles discovered online that were not originally on the map; obstacles that were originally on the map but not found in reality; and obstacles that were originally on the map but whose positions have shifted, i.e., obstacles that have disappeared and then reappeared at new locations. The local environmental perception information received by the robot during execution is the raw perception information; after processing and judgment, local obstacle update information can be extracted to determine whether the multi-scale convex set graph has locally failed.

[0027] The robot runs along the current execution trajectory selected in step 5); during the operation, it acquires local environmental perception information in real time, and records the detected obstacle additions, obstacle offsets, or obstacle disappearances as local obstacle update information. Subsequently, all fine-scale convex set nodes that intersect with the local obstacle update information are identified in the fine-scale convex set graph and recorded as the affected fine-scale node set. The affected fine-scale nodes and their related edges are deleted, and collision-free sampling and fine-scale convex set expansion are performed only within the spatial range of the affected fine-scale node set to locally repair the fine-scale convex set graph.

[0028] 7) Perform local incremental updates on the coarse-scale convex set graph based on the fine-scale update results:

[0029] After completing the fine-scale local repair in step 6), identify all coarse-scale nodes supported by the affected fine-scale nodes; recalculate the convex hulls of the supporting fine-scale convex sets for these coarse-scale nodes, and update the edge relationships between the corresponding coarse-scale nodes. This allows for incremental updates of the coarse-scale convex set graph only within the affected subgraph, while unaffected regions remain unchanged. This step maintains global connectivity without reconstructing the entire graph.

[0030] 8) Determine if the current execution path is invalid; if not, continue execution:

[0031] After completing the fine-scale convex set graph update in step 6), it is determined whether the current execution trajectory is still within the effective free space and effective safe corridor. If the current execution trajectory is not invalid, the robot continues to run along the current execution trajectory; if the current execution trajectory is invalid, it proceeds to step 9) to perform local safe corridor search and topological redundancy fusion. This "determine first, then repair" order can reduce unnecessary replanning overhead.

[0032] 9) When the current execution trajectory fails, perform a local safe corridor search on the updated fine-scale convex set graph, and define a consistent visibility deformation criterion for the corridor level:

[0033] When the current execution trajectory fails, a local convex set sequence search is performed on the updated fine-scale convex set graph from the start to the end of the failed region to obtain a set of local candidate safe corridors. To identify topological redundancy in local candidate safe corridors, this invention extends the consistent visible deformation from the path level to the convex set sequence level. If two convex set sequences have a collision-free common refined convex set sequence, and this common refined convex set sequence satisfies the monotonic index embedding and adjacent intersection alignment conditions, then the two are determined to belong to a fusionable topological redundancy corridor.

[0034] 10) Perform fusion on local candidate safe corridors; if fusion is successful and at least one valid local safe corridor is obtained, then regenerate the execution trajectory:

[0035] The set of local candidate safe corridors obtained in step 9) is subjected to consistent visible deformation discrimination one by one. For local candidate safe corridors that are determined to be topologically redundant, a new representative fused corridor is formed by constructing the convex hull of the corresponding convex set pair; local candidate safe corridors with different topologies are retained respectively. If at least one effective local safe corridor is obtained after fusion, candidate trajectories are regenerated within these local safe corridors, and new execution trajectories are reselected according to the trajectory evaluation cost in step 5).

[0036] 11) When no effective local safety corridor is obtained, i.e., local repair fails, perform global replanning on the updated coarse-scale convex set graph:

[0037] If no effective local safe corridor is obtained in step 10), it means that the current local repair cannot maintain the original traffic topology or cannot restore traffic. In this case, the global candidate safe corridor search is re-executed on the updated coarse-scale convex set graph. Trajectory generation and trajectory selection are performed on the candidate safe corridor set obtained by the re-search to obtain a new execution trajectory, and the robot continues to move to the target position. This forms a "local priority, global backup" replanning strategy.

[0038] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0039] This invention generates safe corridors directly on the convex set graph representation in free space, avoiding the geometric biases and topological limitations caused by corridor expansion based on predefined reference paths in existing technologies. By extending visible deformation and uniform visible deformation to the convex set sequence level, it achieves effective differentiation of safe corridors with different topologies and fusion of redundant corridors, improving the effectiveness of corridor search and selection. At the same time, by constructing an adaptive multi-scale convex set graph, it takes into account both global topology exploration capabilities and local geometric update capabilities, enabling incremental updates without overall reconstruction when the environment changes locally. This improves the efficiency of safe corridor generation, trajectory quality, and adaptability to environmental uncertainties in robot motion planning. Attached Figure Description

[0040] Figure 1 This is a flowchart of the algorithm framework of the present invention.

[0041] Figure 2 This refers to the platform module constructed to implement the method of the present invention in specific implementation. Detailed Implementation

[0042] The present invention will be further illustrated below with reference to the accompanying drawings and embodiments.

[0043] like Figure 1 As shown, the present invention provides a method for generating a homotopy-aware safe corridor for robot motion planning, the overall process of which includes:

[0044] 1. Obtain the prior map of the environment, set the maximum size of the sampling convex set, and the robot's start and end points;

[0045] 2. Construct a multi-scale convex set graph based on the prior map;

[0046] 3. Search for several candidate safe corridors on the coarse-scale convex set graph based on the start and end points, generate candidate trajectories based on the candidate safe corridors, and select the current execution trajectory;

[0047] 4. Determine if the robot has reached the destination. If the robot has reached the destination, the algorithm ends; if the robot has not reached the destination, repeat steps 5-8.

[0048] 5. During robot execution, acquire local environmental perception information in real time, continuously update obstacle information near the robot's current position, and locally update the multi-scale convex set graph accordingly;

[0049] 6. Continue execution if the current trajectory is not invalid. If the current trajectory is invalid, first perform a local search on the updated fine-scale convex set graph and merge topologically similar local safe corridors.

[0050] 7. If the partial repair is successful, the trajectory will be regenerated and executed based on the repaired safe corridor;

[0051] 8. If local repair fails, a global research is performed on the updated coarse-scale convex set graph, and the trajectory is regenerated and executed based on the candidate safe corridors.

[0052] In practical implementation, the homotopy-aware safe corridor generation method for robot motion planning provided by this invention includes the following steps:

[0053] Step 1: Obtain a prior map of the environment and set the maximum size or resolution of the fine-scale sampling convex set. And set the robot's start and end points. Among them, Used to control the local geometric accuracy of fine-scale convex sets; The smaller the size, the more detailed the description of the fine-scale free space, but the higher the corresponding composition cost.

[0054] Step two: Construct a fine-scale convex set graph based on the prior map. Specifically, in free space... Collision-free sampling is performed within the area, and each sampling point is extended to the surrounding collision-free region into several fine-scale convex sets. (In practice, simple convex sets can be used, such as 2D squares, 3D cubes, etc.). Using fine-scale convex sets as graph nodes and the existence of non-empty overlaps between fine-scale convex sets as the edge connection condition, a fine-scale convex set graph is constructed. If two fine-scale convex sets have a non-empty overlap, then an edge relationship is established between the corresponding nodes. Fine-scale convex set graphs are used to finely characterize the local geometry of free space and provide underlying support for subsequent local repairs.

[0055] Step 3: Construct a coarse-scale convex set graph based on the fine-scale convex set graph. Specifically, select root nodes that are not visible to each other from the fine-scale convex set graph, and perform breadth-first traversal aggregation on the fine-scale subgraph starting from each root node. For the fine-scale convex sets that aggregate to the same coarse-scale node, find the union and then take the convex hull to form the coarse-scale convex set.

[0056]

[0057] in, Indicates support for coarse-scale nodes A set of fine-scale node indexes This represents the convex hull operation. Further, using coarse-scale convex sets as nodes and determining whether there is a non-empty overlap between coarse-scale convex sets as the connection condition, a coarse-scale convex set graph is constructed. A multi-scale convex graph is constructed by combining fine-scale and coarse-scale convex graphs. The fine-scale convex graph preserves local geometric information, while the coarse-scale convex graph represents the global connectivity topology and reduces the complexity of the global search.

[0058] Step four: Connect the start and end points to a coarse-scale convex set graph, and search for candidate safe corridors on the coarse-scale convex set graph. During the search, convex set nodes containing both the start and end points are designated as start point nodes. and the endpoint Perform a breadth-first search on the coarse-scale convex graph to obtain one or more candidate convex set sequences. Each candidate security corridor can be represented as... ,in, This represents the m-th candidate safe corridor. This represents a convex set in the security corridor, corresponding to a line on a coarse-scale convex set diagram from... arrive The graph path. This search process can be written as...

[0059]

[0060] in, This represents the set of candidate convex set sequences from the source node to the target node. Since the coarse-scale convex set graph is constructed based on visibility, different candidate safe corridors typically correspond to different visibility deformation categories, which can compactly cover multiple topologically different travel methods.

[0061] Step 5: Generate candidate trajectories based on the candidate safety corridors and select the current execution trajectory. For each candidate safety corridor obtained in Step 4, existing technical solutions can be used to perform trajectory optimization within it (e.g., Z. Wang, X. Zhou, C. Xu, and F. Gao, “Geometrically constrained trajectory optimization for multicopters,” IEEE Transactions on Robotics, vol. 38, no. 5, pp. 3259–3278, 2022.) to obtain a set of candidate trajectories. In this embodiment, a trajectory optimizer is preferably used to generate trajectories that satisfy geometric and collision constraints; for multiple candidate trajectories, a user-defined performance cost function is used. Select the current execution trajectory. The selection process can be written as follows:

[0062]

[0063] in, This represents the m-th candidate trajectory. This represents the selected current execution trajectory. Cost function. It can be trajectory duration, trajectory length, average speed, smoothness, or a combination thereof.

[0064] Step Six: During the robot's execution of the current trajectory, local sensors are used to acquire local environmental perception information in real time, and obstacle information near the robot's current position is continuously updated accordingly. Information on the addition, shift, or disappearance of detected obstacles is used as local obstacle update information. When local obstacle update information arrives, only the affected area is updated with a multi-scale convex set map, without needing to reconstruct the entire map, thus improving online planning efficiency.

[0065] Step 7: Update the fine-scale convex set graph based on the local obstacle update information. First, identify all objects in the fine-scale convex set graph that are related to the local obstacle update information. The set of fine-scale nodes that intersect is defined as the set of affected fine-scale nodes.

[0066]

[0067] set The nodes and their associated edges in the graph are removed from the fine-scale convex set. Only in the failed regions are new fine-scale convex sets resampled and expanded to restore the local connectivity of the fine-scale graph.

[0068] Step 8: Synchronously update the coarse-scale convex set graph based on the fine-scale update results. If a certain coarse-scale node... Among the supported fine-scale nodes, there are affected nodes, i.e., satisfying...

[0069]

[0070] If the coarse-scale node is considered to be affected, its corresponding coarse-scale convex set is recalculated. After the update, only the affected coarse-scale subgraph changes, while the unaffected part remains unchanged, thus maintaining the stable representation of the overall free space topology.

[0071] Step nine: Determine if the current execution trajectory is invalid. If the current execution trajectory is still valid in the updated free space, continue executing the current trajectory and return to step six; if the current execution trajectory is invalid, proceed to step ten, performing local search and local repair on the updated fine-scale convex set graph. This strategy is similar to... Figure 1 The corresponding decision box is "Is the current trajectory invalid?".

[0072] Step 10: Perform a local search on the updated fine-scale convex set graph and fuse topologically similar local safe corridors. Since the fine-scale convex set graph is updated earlier than the coarse-scale convex set graph, local candidate safe corridors are searched preferentially at the fine scale. To determine whether different local candidate safe corridors belong to topological redundancy, this invention extends visible deformation and consistent visible deformation from the path level to the convex set sequence level. For topological consistency between different scales, it is defined that if two candidate convex set sequences...

[0073]

[0074] There exists a collision-free common refinement sequence and satisfy

[0075]

[0076]

[0077] Then these two convex set sequences are considered to satisfy the visible deformation relation, denoted as The first condition requires that any convex set in the two candidate convex set sequences be contained in at least one convex set in a common refined sequence; the second condition requires that any convex set in the common refined sequence contain at least one convex set from each of the two candidate convex set sequences. This relationship guarantees the topological consistency between fine-scale and coarse-scale security corridors. For redundant fusion between local candidate security corridors, this invention employs a consistent visible deformation criterion. If two local candidate convex set sequences satisfy... This indicates that both can be jointly refined while maintaining the consistency of the convex set order and the alignment of adjacent intersections, thus belonging to a fusionable topological redundancy safe corridor. More specifically, if a collision-free refinement sequence exists... and index-coupled sequences and satisfy

[0078]

[0079]

[0080]

[0081] If two local candidate safe corridors satisfy the condition of consistent visible deformation, they can be further fused. The first condition requires that the index size of the index coupling sequence does not decrease, and at least one index must increase each time. The second condition requires that the convex set in the collision-free refinement sequence contains the convex sets of the corresponding indices in the two candidate safe corridors. The third condition requires that the intersection of two adjacent convex sets in the collision-free refinement sequence contains the intersection of adjacent convex sets of the corresponding indices in the two candidate safe corridors. When the local candidate safe corridors satisfy the condition of consistent visible deformation, the fused representative safe corridor can be constructed according to the convex hull of the corresponding convex set pairs. Its k-th fused convex set can be written as...

[0082]

[0083] This fusion operation reduces the number of geometrically different but topologically equivalent redundant corridors, thereby reducing the computational burden of subsequent sequential trajectory optimization. If at least one effective local safe corridor is obtained after local fusion, candidate trajectories are regenerated based on these safe corridors, and the currently executed trajectory is updated. If local repair fails, i.e., no effective local safe corridor is obtained, proceed to step eleven.

[0084] Step 11: Perform a global re-search on the updated coarse-scale convex set graph. At this point, a new search is performed on the updated coarse-scale convex set graph from... arrive The candidate safe corridor set is determined, and trajectory generation and selection are performed again to obtain the new current execution trajectory. This step corresponds to... Figure 1 The branch "Perform a global research on the updated coarse-scale convex set graph" is then executed. After obtaining the new trajectory, the robot continues execution and repeats steps six through eleven until the robot reaches the destination.

[0085] Figure 2 The experimental platform shown comprises three main modules: an algorithm module, a communication and computing module, and an execution layer module. The algorithm module contains the robot's motion planning algorithm and provides an API interface for researchers to modify the algorithm and parameters. The communication and computing module includes an onboard computer or ground station host, primarily providing a computing terminal and data communication medium. The execution layer module includes the Optitrack positioning system and robot platforms such as UAVs and unmanned vehicles. The Optitrack positioning system primarily provides the individual robot position information required by the algorithm.

[0086] This invention provides a homotopy-aware safe corridor generation method for robot motion planning. It directly searches for candidate safe corridors composed of convex set sequences on a convex set graph in free space, thereby reducing the geometric bias and topological constraints imposed by reference paths. By extending visible deformation and uniform visible deformation from the path level to the convex set sequence level, it can identify candidate corridors with different topologies and fuse topologically redundant corridors, achieving topological discrimination at the corridor level. Simultaneously, this invention designs an adaptive multi-scale convex set graph, where a fine-scale convex set graph is used for fine-grained local geometric representation and local repair, and a coarse-scale convex set graph is used for global topological exploration and compact search. A local incremental update mechanism avoids global reconstruction of the entire graph when the environment changes. Based on the above technologies, this invention can improve the efficiency of safe corridor generation, the quality of trajectory optimization, and the system's adaptability to environmental uncertainties. This invention is applicable to path planning, trajectory generation, and online replanning for autonomous platforms such as ground mobile robots and UAVs in complex obstacle environments. This method can generate and maintain safe corridors in multi-traffic topology environments and scenarios with local map changes. It is suitable for applications such as warehousing and logistics, autonomous driving, park inspection, and search and rescue. It can achieve the construction and maintenance of safe corridors without predefined reference paths in complex obstacle environments and under conditions of local map updates. With the development of intelligent manufacturing, unmanned systems, and the low-altitude economy, this invention has good application prospects.

[0087] It should be noted that the purpose of disclosing the embodiments is to help further understand the present invention. However, those skilled in the art will understand that various substitutions and modifications are possible without departing from the scope of the present invention and the appended claims. Therefore, the present invention should not be limited to the content disclosed in the embodiments, and the scope of protection of the present invention is defined by the scope of the claims.

Claims

1. A method for generating a safe corridor based on homotopy perception in robot motion planning, characterized in that, The method is designed for collision-free motion planning tasks of robots in complex environments and includes the following steps: 1) Establish the configuration space model, obstacle set, and free space set for robot motion planning; define the free space set as the feasible area in the configuration space after removing obstacles, and set the start and end points of the robot motion planning route; Based on the prior map, construct an adaptive multi-scale convex set graph representation of free space, including coarse-scale convex set graphs and fine-scale convex set graphs; 2) Construct a fine-scale convex set graph within the free space set: Collision-free sampling is performed within the free space set, and each sampling point is extended to the surrounding collision-free region as a fine-scale convex set. Then, using fine-scale convex sets as graph nodes and the condition that two fine-scale convex sets have non-empty overlap as the connection condition, a fine-scale convex set graph is constructed to finely describe the local geometry of free space. 3) Construct a coarse-scale convex set graph based on the fine-scale convex set graph, and form an adaptive multi-scale convex set graph: Select root nodes that are not visible to each other from the fine-scale convex graph, and perform traversal aggregation on the fine-scale convex graph starting from the root nodes. Merge the fine-scale convex sets that belong to the same aggregation result and take the convex hull. If the convex hull satisfies the safety condition of no collision, the corresponding coarse-scale convex set is obtained. Then, use the coarse-scale convex set as graph nodes and use the non-empty overlap between coarse-scale convex sets as the edge connection condition to construct the coarse-scale convex graph. The fine-scale convex graph and the coarse-scale convex graph together constitute the adaptive multi-scale convex graph. Among them, the fine-scale convex graph is used for local geometry preservation and local update, and the coarse-scale convex graph is used for global connectivity expression and corridor search. 4) Connect the starting point and ending point of the robot motion planning route to the multi-scale convex graph, and search for candidate safe corridors for the robot motion route on the coarse-scale convex graph. 5) Generate candidate trajectories based on candidate safety corridors, and select the current execution trajectory: During the robot's online trajectory execution, the local environment perception results are acquired in real time, and the multi-scale convex set map is locally updated; including steps 6) to 7). 6) During robot execution, receive local environmental perception information and identify affected fine-scale convex sets; including: Information on newly added obstacles, obstacle displacement, or obstacle correction is used as local environmental perception information; In the fine-scale convex set graph, identify all fine-scale convex set nodes that intersect with the local obstacle update information, and denote them as the set of affected fine-scale nodes; Delete the affected fine-scale nodes and related edges, and only re-sampling and fine-scale convex set expansion within the spatial range where the affected fine-scale node set is located, thereby locally repairing the fine-scale convex set graph; 7) Perform local incremental updates on the coarse-scale convex set graph based on the update results of the fine-scale convex set graph, including: First, identify all coarse-scale nodes supported by affected fine-scale nodes; then recalculate the convex hulls supporting the fine-scale convex sets for the coarse-scale nodes and update the edge relationships between the corresponding coarse-scale nodes. That is, the incremental update of the coarse-scale convex set graph is only completed within the affected subgraph, so that global connectivity is maintained without rebuilding the entire graph. 8) Determine if the current execution path is invalid; if not, continue execution; 9) When the current execution trajectory fails, a local safe corridor search is performed on the updated fine-scale convex set graph to obtain a set of local candidate safe corridors; and a consistent visible deformation criterion for the corridor level is defined to identify fusionable topological redundancy corridors. 10) Perform fusion on local candidate safe corridors; if fusion is successful and at least one valid local safe corridor is obtained, then regenerate the execution trajectory; 11) When no effective local safety corridor is obtained, global replanning is performed on the updated coarse-scale convex set graph to obtain a new execution trajectory.

2. The method for generating a homotopy-aware safe corridor for robot motion planning as described in claim 1, characterized in that, In step 1), the configuration space is established based on the robot's motion dimension, and a set of convex obstacles is obtained based on the environment map. When there are non-convex obstacles in the environment, the non-convex obstacles are decomposed or approximated as multiple convex obstacles.

3. The method for generating a homotopy-aware safe corridor for robot motion planning as described in claim 1, characterized in that, Step 2) Construct the fine-scale convex set graph, where the upper bound of the scale of each fine-scale convex set is determined by a preset resolution. Constraints; Constructing a fine-scale convex set graph is represented as follows: ,in, This represents the set of nodes in a fine-scale convex graph structure. It represents the set of edge relations in a fine-scale convex graph structure.

4. The method for generating a homotopy-aware safe corridor for robot motion planning as described in claim 3, characterized in that, Step 3) Construct a coarse-scale convex set graph as follows: ,in, This represents the set of nodes in a coarse-scale convex graph structure. This represents the set of edge relations in a coarse-scale convex graph structure.

5. The method for generating a homotopy-aware safe corridor for robot motion planning as described in claim 1, characterized in that, Step 4) Search for candidate safe corridors on the coarse-scale convex set graph, specifically including: Connect the start point and the end point to the multi-scale convex set nodes, and denote them as the start point node and the end point node, respectively. On a coarse-scale convex set graph, one or more candidate convex set sequences are searched from the starting node to the ending node to obtain a set of candidate safe corridors; each candidate safe corridor is represented as a sequence of convex sets arranged in order, and the sequence of convex sets corresponds to a feasible corridor from the starting point to the ending point.

6. The method for generating a homotopy-aware safe corridor for robot motion planning as described in claim 1, characterized in that, Step 5) Generate candidate trajectories based on candidate safety corridors, and select the current execution trajectory, including: For one or more candidate safety corridors, perform trajectory optimization within each candidate corridor to obtain the corresponding candidate trajectory set; Then, based on the set trajectory evaluation cost function, the trajectory with the best cost is selected from the candidate trajectory set as the current execution trajectory; the trajectory evaluation cost function setting includes one or a combination of trajectory duration, trajectory length, average speed, and smoothness.

7. The method for generating a homotopy-aware safe corridor for robot motion planning as described in claim 1, characterized in that, In step 8), determining whether the current execution trajectory is invalid means that if the current execution trajectory is still within the valid free space and the valid safe corridor, then it is not invalid.

8. The method for generating a homotopy-aware safe corridor for robot motion planning as described in claim 1, characterized in that, In step 9), the consistent visible deformation criterion for the corridor level is as follows: if two convex set sequences have a collision-free common refined convex set sequence, and the common refined convex set sequence satisfies the monotonic index embedding and adjacent intersection alignment conditions, then the two are determined to belong to a topologically redundant corridor that can be merged.

9. The method for generating a homotopy-aware safe corridor for robot motion planning as described in claim 8, characterized in that, In step 10), the local candidate safe corridor set obtained in step 9) is specifically judged by consistent visible deformation. For local candidate safe corridors that are topologically redundant, a new representative fusion corridor is formed by constructing the convex hull of the corresponding convex set pair. Local candidate safe corridors with different topologies are retained respectively. If at least one effective local safe corridor is obtained after fusion, a candidate trajectory is regenerated inside the local safe corridor, and a new execution trajectory is reselected according to the trajectory evaluation cost.

Images