Mobile robot path planning method for homotopy space sampling FMT*

Abstract

The invention provides a mobile robot path planning method based on homotopy space sampling FMT *. The method comprises the following steps: S1, initializing a map; s2, establishing a generalized Voronoi diagram; s3, extracting feature nodes; s4, constructing a feature visibility graph; s5, searching an initial heuristic path; s6, feature unit decomposition; s7, constructing an adjacent graph; s8, solving a heuristic path; s9, homotopy space calculation is carried out; s10, carrying out non-uniform sampling; s11, a sampling set # imgabs0 #, an exploration set # imgabs1 # and an explored set # imgabs2 # are constructed; s12, searching nodes of a sampling set # imgabs3 #, and forming a set # imgabs4 #; s13, exploring the midpoints of the # imgabs 5 # to form a set # imgabs 6 #; s14, the cost from the point # imgabs7 # to the point # imgabs8 # is calculated, and progressive optimization of the algorithm is achieved; s15, whether obstacles exist in the # imgabs9 and the # imgabs10 or not is judged, and the result is processed; s16, the required Z is put into the # imgabs 12 # from the # imgabs 11 #; s17, if # imgabs13 # is a target point, returning to the path solution, and if not, returning to S7 and continuing; s18, the robot processes the feasible path solution; the convergence speed of the algorithm is higher than that of the FMT *, and the optimal solution can be quickly obtained.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot path planning, in particular to a mobile robot path planning method based on homotopic space sampling FMT*. Background Art

[0002] Nowadays, automatic navigation has become one of the important functions of intelligent robots in providing services, and is widely used in fields such as autonomous driving, aerial drones, and service robots. Path planning, as the core part of navigation, aims to enable the robot to find a collision-free and safe path from the starting point to the end point within a given operating space. This requires not only that the robot can find an obstacle-free path in a complex environment, but also that the path be optimized to be as short and smooth as possible. Therefore, the development of intelligent robots and unmanned operation technology depends largely on whether the motion planning problem can be solved efficiently.

[0003] The field of path planning has developed rapidly, and many effective planning algorithms have been produced. Sampling-based algorithms, such as rapidly exploring random trees (RRT) and probabilistic roadmaps (PRM), as well as their various variants, have attracted increasing attention due to their ability to effectively handle constrained dynamics and high-dimensional space planning problems. To further improve the path quality, researchers have proposed methods with asymptotic optimality, such as RRT* and PRM*, which have made sampling algorithms one of the most widely used algorithms. However, a major limitation of sampling algorithms is that they converge to the optimal solution slowly and require a large number of iterations or samples to gradually approach the optimal solution. In practical applications, such as drones or mobile robots, in order to save time and energy, it is usually necessary to quickly calculate a shorter path to ensure efficient execution of the task.

[0004] Fast Matching Tree (FMT*) is a variant of RRT*. FMT* obtains a batch of samples in space and lazily expands them in a tree-like manner. This method does not require rewiring, so it converges to the optimal solution faster. However, FMT* wastes a lot of computing time on irrelevant spaces when exploring samples. Group Marching Tree (GMT*) proposed by B. Ichter et al. uses the parallel computing power of GPU to process the exploration process of FMT*, which speeds up the exploration speed of FMT*, but this method places higher requirements on hardware. Bidirectional FMT* (BFMT*) proposed by J. Starek et al. explores samples from both the starting point and the target point at the same time, which improves the exploration efficiency, but still does not avoid the generation of irrelevant samples. Informed-RRT* adopts an informed sampling strategy to limit sampling to a heuristic elliptical area based on path length, avoiding a large number of unnecessary samples. Informed Anytime FMT* (IAFMT*) also adopts this informed sampling strategy, but in complex environments, due to the long path length, the performance of this strategy is poor. The ST-FMT* proposed by Z. Wu et al. uses the Voronoi diagram to find the initial path and performs non-uniform sampling around the path, reducing irrelevant samples. However, due to its dependence on the Voronoi diagram, it may produce an initial path that is not homotopic with the optimal path, ultimately causing the algorithm to fail to converge to the optimal path. Summary of the invention

[0005] The present invention proposes a mobile robot path planning method based on homology space sampling FMT*, which overcomes the shortcomings of the existing path planning algorithm FMT*. The algorithm converges faster than FMT* and can quickly obtain the optimal solution, which has advantages in single query planning.

[0006] The present invention adopts the following technical solutions.

[0007] A mobile robot path planning method based on homotopy space sampling FMT* comprises the following steps:

[0008] Step S1, map initialization: all known obstacles in the environment are discretized with a uniformly distributed set of points, and assuming that the number of obstacles is k, the set {C i},Right now

[0009] Step S2, using an incremental construction method to establish a generalized Voronoi diagram, namely, GVG, which is composed of generalized Voronoi edges and vertices;

[0010] Step S3, extracting feature nodes: obtaining the distance d from the nearest obstacle through the GVG establishment process, taking it as the radius R of the GVG vertex, comparing the positional relationship between the GVG vertices and the size of the radius R, extracting the GVG vertex with a larger radius as the feature node;

[0011] Step S4: constructing a feature visibility graph: perform visibility detection on each pair of feature nodes, add edges between visible feature nodes, and these edges and visible feature nodes form a feature visibility graph T. F ;

[0012] Step S5, find the initial heuristic path: set the initial position and target position of the robot, and find the initial heuristic path P in the feature visibility graph init , realize the homology between the initial heuristic path and the optimal path;

[0013] Step S6, feature unit decomposition: according to the positional relationship between the state point and all feature nodes in the free space, the free space is decomposed into a number of feature units, each feature unit having only one feature node;

[0014] Step S7, adjacency graph construction: determine whether two feature units are adjacent, add edges between corresponding feature nodes in adjacent feature units, and these edges and adjacent feature nodes form an adjacency graph T A ;

[0015] Step S8, solving the heuristic path: starting from the initial position of the initial heuristic path, use the A* optimal graph search method in the adjacency graph to find the local path solution P to the next path point part ;Search for the local path solution to reach the next path point in sequence until reaching the target position, and concatenate all local path solutions into a heuristic path P connect ,The heuristic path is implemented by the adjacent feature nodes;

[0016] Step S9, homotopy space calculation: construct the homotopy space S of the optimal path according to the heuristic path, use the homotopy space as the sampling space, and realize that the path in the sampling space is homotopic with the optimal path;

[0017] Step S10, non-uniform sampling: set the total number of samples N, adjust the sampling coefficient according to the area of ​​each feature unit constituting the sampling space, perform random sampling in each feature unit according to the sampling coefficient, and obtain the sampling set V after taking the union unvisited ;

[0018] Step S11: construct a sampling set V unvisited , Explore Collection V open 、Explored Set V closed , the target point x of the robot path goal Put in Vunvisited , the initial point x start Add to Exploration Set V open , the cost is initialized to 0, denoted as c(x start )=0, explored set

[0019] Step S12: Find the exploration set V open The point with the minimum cost is taken as node Z, with Z as the center, r n Search sampling set V within the radius unvisited The nodes form a set X near , with the function Near(V unvisited ,Z,r n )express;

[0020] Step S13: for each point x∈X near , with x as the center and radius r n Explore V open midpoint, forming set Y near , with the function Near(V open ,x,r n )express;

[0021] Step S14, calculate all points y∈Y near The cost to get to point x, select the point y with the minimum cost min , expressed as Minimize the cost from each node to the child node in the tree, so as to achieve the asymptotic optimality of the algorithm;

[0022] Step S15: Determine y min Is there an obstacle with x? If not, move x from V unvisited Put in V open The cost is c(x)=c(y min )+Cost(y min ,x),y min is the parent node of x;

[0023] Step S16: Exploration completed near Then, change Z from V open Put in V closed ;

[0024] Step S17, determine whether Z is the target point, if so, return the path solution consisting of the points in the explored set, if not, return to step S12 to continue;

[0025] Step S18: The robot receives the feasible path solution planned by the algorithm, and converts the path in the feasible path solution into the required linear velocity and angular velocity, controls the robot to move to the target position, and the path planning is completed.

[0026] The specific implementation method of extracting feature nodes in step S3 is:

[0027] Distance d i (x) represents the distance from point x to the i-th obstacle C i The distance is defined as: Define the direction from point x to c0, by the gradient Indicates that c0 represents the point x to C i The closest point to

[0028] If there are many obstacles around point x, the distance from point x to the nearest obstacle is defined as This distance is taken as the obstacle-free radius R of point x;

[0029] Put all GVG vertices into a GVG vertex set, which contains the coordinate information and radius R of each GVG vertex;

[0030] Start the extraction operation: extract the GVG vertex with the largest radius from the GVG vertex set as the feature node, calculate the distance from other GVG vertices in the set to the GVG vertex, and if the distance is less than the radius R of the GVG vertex, remove the corresponding other GVG vertices from the GVG vertex set to complete an extraction operation;

[0031] Repeat the above extraction operation until the GVG vertex set is an empty set. Assuming that m feature nodes are extracted, the feature node set {F j},Right now

[0032] The step S5 of finding the initial heuristic path will use the A* optimal graph search method to find the current optimal solution in the feature visibility graph. According to the connectivity of the feature visibility graph, the current solution will be a homotopic solution with the global optimal path.

[0033] The specific implementation method of the characteristic unit decomposition in step S6 is:

[0034] Represents point x to the jth feature node F j The characteristic distance is defined as Where R is the feature node F j The barrier-free radius;

[0035] For a point x in the free space, the characteristic distances from point x to all characteristic nodes are calculated respectively, and point x is defined as being in the characteristic cell represented by the characteristic node with the smallest characteristic distance; all state points in the free space are divided to obtain m characteristic cells, and the characteristic cell set is represented as {Cell j},Right now The intersection of two feature units is an empty set, and the union of all feature units is the entire free space. The feature units provide a concise representation of the topological relationship of environmental information.

[0036] The specific implementation method of the homotopy space calculation in step S9 is: the heuristic path is composed of adjacent feature nodes, the union of the feature units where these feature nodes are located is taken to form the homotopy space of the optimal path, and the homotopy space is used as the sampling space;

[0037] The method performs sampling in the homotopy space of the optimal path, thereby narrowing the sampling range, improving the utilization rate of sampling points, and reducing the amount of calculation to improve the calculation efficiency of the path planning algorithm.

[0038] The specific implementation method of the non-uniform sampling of step S10 is: calculate the area A of each feature unit constituting the sampling space, that is, the number of grids contained in the feature unit, and record the area of ​​the jth feature unit as A j , the sum of the areas of all characteristic units constituting the sampling space is A s The ratio to the number of characteristic units is defined as the average area Then the sampling coefficient calculation method in the jth feature unit is:

[0039]

[0040] Where μ is the reference sampling coefficient, which is determined by the total number of samples N;

[0041] The method adjusts the distribution of sampling points between narrow areas and wide areas, thereby accelerating the convergence speed of the path planning algorithm without increasing the amount of additional calculations.

[0042] Compared with the prior art, the present invention has the following beneficial effects:

[0043] Compared with ST-FMT*, FMT*, Informed-RRT*, RRT*, and PRM* algorithms, the algorithm proposed in this invention has the following advantages:

[0044] (1) Using the Voronoi diagram to find the initial path may cause the problem that the initial path is not homotopic with the optimal path. The present invention proposes a new feature visibility graph. This route map is evenly distributed relative to obstacles in space and can find an initial heuristic path that is homotopic with the optimal path, ensuring that the algorithm can converge to the optimal state.

[0045] (2) The present invention proposes a non-uniform sampling algorithm that combines heuristic path finding and homology space calculation. The samples generated by the algorithm are all in the homology space of the optimal path, and the sample density can be adaptively adjusted according to the environment, which greatly reduces the generation of irrelevant samples and the calculation time of the algorithm to find the optimal path solution. When solving the multi-query planning problem, the map preprocessing operation only needs to be performed once, which greatly improves the calculation efficiency when solving the multi-query planning problem.

[0046] (3) The present invention performs non-uniform sampling in the homotopy space after preprocessing the map. It can be used in combination with any sampling-based planning algorithm to improve its computational efficiency and convergence performance. It has strong applicability and is an algorithm with completeness and asymptotic optimality. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Attached Figure 1 It is a schematic diagram of the process of the present invention;

[0048] Attached Figure 2 It is a schematic diagram for explaining the algorithm principle of the present invention;

[0049] Attached Figure 3 It is a schematic diagram of feature node extraction of the present invention;

[0050] Attached Figure 4 It is a schematic diagram of the decomposition of the characteristic unit of the present invention;

[0051] Attached Figure 5 It is a schematic diagram of a simulation result of a static scene of the present invention;

[0052] Attached Figure 6 It is a schematic diagram of the simulation results of the static scene 2 of the present invention. DETAILED DESCRIPTION

[0053] The technical solution of the present invention is described in detail below in conjunction with the accompanying drawings.

[0054] like Figure 1 As shown, a mobile robot path planning method based on homotopy space sampling FMT* includes the following steps:

[0055] Step S1, map initialization: all known obstacles in the environment are discretized with a uniformly distributed set of points, and assuming that the number of obstacles is k, the set {C i},Right now

[0056] Step S2: Using the incremental construction method to establish a generalized Voronoi diagram, namely, GVG, which is composed of generalized Voronoi edges and vertices; Figure 2 As shown in area a;

[0057] Step S3, extracting feature nodes: obtain the distance d from the nearest obstacle through the GVG establishment process, use it as the radius R of the GVG vertex, compare the positional relationship between the GVG vertices and the size of the radius R, and extract the GVG vertex with a larger radius as the feature node; Figure 2 As shown in area a;

[0058] Step S4: constructing a feature visibility graph: perform visibility detection on each pair of feature nodes, add edges between visible feature nodes, and these edges and visible feature nodes form a feature visibility graph T. F ;like Figure 2 As shown in area b;

[0059] Step S5, find the initial heuristic path: set the initial position and target position of the robot, and find the initial heuristic path P in the feature visibility graph init , realize the homology between the initial heuristic path and the optimal path; Figure 2 As shown in area b;

[0060] Step S6, feature unit decomposition: according to the positional relationship between the state point and all feature nodes in the free space, the free space is decomposed into a number of feature units, each feature unit has and only has one feature node; Figure 2 As shown in the c area;

[0061] Step S7, adjacency graph construction: determine whether two feature units are adjacent, add edges between corresponding feature nodes in adjacent feature units, and these edges and adjacent feature nodes form an adjacency graph T A ;like Figure 2 As shown in the d area;

[0062] Step S8, solving the heuristic path: starting from the initial position of the initial heuristic path, use the A* optimal graph search method in the adjacency graph to find the local path solution P to the next path point part ;Search for the local path solution to reach the next path point in sequence until reaching the target position, and concatenate all local path solutions into a heuristic path P connect , the heuristic path is composed of adjacent feature nodes; Figure 2 As shown in the e area;

[0063] Step S9, homology space calculation: construct the homology space S of the optimal path according to the heuristic path, use the homology space as the sampling space, and realize that the path in the sampling space is homological with the optimal path; Figure 2 As shown in the e area;

[0064] Step S10, non-uniform sampling: set the total number of samples N, adjust the sampling coefficient according to the area of ​​each feature unit constituting the sampling space, perform random sampling in each feature unit according to the sampling coefficient, and obtain the sampling set V after taking the union unvisited ;like Figure 2 As shown in the f area;

[0065] Step S11: construct a sampling set V unvisited , Explore Collection V open 、Explored Set V closed , the target point x of the robot path goal Put in V unvisited , the initial point x start Add to Exploration Set V open , the cost is initialized to 0, denoted as c(x start )=0, explored set

[0066] Step S12: Find the exploration set V open The point with the minimum cost is taken as node Z, with Z as the center, r n Search sampling set V within the radius unvisited The nodes form a set X near , with the function Near(V unvisited ,Z,r n )express;

[0067] Step S13: for each point x∈X near , with x as the center and radius r n Explore V open midpoint, forming set Y near , with the function Near(V open ,x,r n )express;

[0068] Step S14, calculate all points y∈Y near The cost to get to point x, select the point y with the minimum cost min , expressed as Minimize the cost from each node to the child node in the tree, so as to achieve the asymptotic optimality of the algorithm;

[0069] Step S15: Determine y min Is there an obstacle with x? If not, move x from V unvisited Put in V open The cost is c(x)=c(y min )+Cost(y min ,x),y min is the parent node of x;

[0070] Step S16: Exploration completednear Then, change Z from V open Put in V closed ;

[0071] Step S17, determine whether Z is the target point, if so, return the path solution consisting of the points in the explored set, if not, return to step S12 to continue;

[0072] Step S18: The robot receives the feasible path solution planned by the algorithm, and converts the path in the feasible path solution into the required linear velocity and angular velocity, controls the robot to move to the target position, and the path planning is completed.

[0073] The specific implementation method of extracting feature nodes in step S3 is:

[0074] Distance d i (x) represents the distance from point x to the i-th obstacle C i The distance is defined as: Define the direction from point x to c0, by the gradient Indicates that c0 represents the point x to C i The closest point to

[0075] If there are many obstacles around point x, the distance from point x to the nearest obstacle is defined as This distance is taken as the obstacle-free radius R of point x;

[0076] Put all GVG vertices into a GVG vertex set, which contains the coordinate information and radius R of each GVG vertex;

[0077] Start the extraction operation: extract the GVG vertex with the largest radius from the GVG vertex set as the feature node, calculate the distance from other GVG vertices in the set to the GVG vertex, if the distance is less than the radius R of the GVG vertex, remove the corresponding other GVG vertices from the GVG vertex set, and complete an extraction operation, such as Figure 3 As shown in area a;

[0078] Repeat the above extraction operation until the GVG vertex set is an empty set. Assuming that m feature nodes are extracted, the feature node set {F j},Right now like Figure 3 As shown in area b.

[0079] The initial heuristic path search in step S5 uses the A* optimal graph search method to find the current optimal solution in the feature visibility graph. According to the connectivity of the feature visibility graph, the current solution will be a homotopic solution with the global optimal path, such as Figure 2 As shown in area b.

[0080] The specific implementation method of the characteristic unit decomposition in step S6 is:

[0081] Represents point x to the jth feature node F j The characteristic distance is defined as Where R is the feature node F j The barrier-free radius;

[0082] For a point x in the free space, the characteristic distances from point x to all characteristic nodes are calculated respectively, and point x is defined as being in the characteristic cell represented by the characteristic node with the smallest characteristic distance; all state points in the free space are divided to obtain m characteristic cells, and the characteristic cell set is represented as {Cell j},Right now The intersection of two feature units is an empty set, and the union of all feature units is the entire free space. Feature units provide a concise representation of the topological relationship of environmental information, such as Figure 4 shown.

[0083] The specific implementation method of the homotopy space calculation in step S9 is: the heuristic path is composed of adjacent feature nodes, and the union of the feature units of these feature nodes is taken to form the homotopy space of the optimal path, and the homotopy space is used as the sampling space, such as Figure 2 As shown in the e area;

[0084] The method performs sampling in the homotopy space of the optimal path, thereby narrowing the sampling range, improving the utilization rate of sampling points, and reducing the amount of calculation to improve the calculation efficiency of the path planning algorithm.

[0085] The specific implementation method of the non-uniform sampling of step S10 is: calculate the area A of each feature unit constituting the sampling space, that is, the number of grids contained in the feature unit, and record the area of ​​the jth feature unit as A j , the sum of the areas of all characteristic units constituting the sampling space is A s The ratio to the number of characteristic units is defined as the average area Then the sampling coefficient calculation method in the jth feature unit is:

[0086]

[0087] Where μ is the base sampling coefficient, which is determined by the total number of samples N, such as Figure 2 As shown in the f area;

[0088] The method adjusts the distribution of sampling points between narrow areas and wide areas, thereby accelerating the convergence speed of the path planning algorithm without increasing the amount of additional calculations.

[0089] Example:

[0090] In this embodiment, the complete pseudo code of the algorithm used is as follows:

[0091]

[0092] Among them, lines 1-12 in Algorithm 1 are the improved parts of the improved FMT* algorithm adopted by the present invention compared with the original FMT* algorithm.

[0093] The following is a detailed description of the implementation of the present invention using specific experiments. The present invention provides a complete and asymptotically optimal motion planning algorithm, and its effectiveness is mainly verified by simulation experiments. The specific simulation settings are as follows:

[0094] Simulation experiment:

[0095] The simulation experiments were carried out in MATLAB R2022b software.

[0096] (1) Static scene 1

[0097] The size of the simulated static scene map is X=Y=500m, and all obstacles are static obstacles. The starting point is (460, 55) and the target point is (40, 465). Figure 5 As shown, it is a simulation map of static scene 1, point S represents the starting point, G represents the target point, the cyan line represents the tree structure, the green point represents the sampling point, and the red line represents the path planned by the algorithm proposed in the present invention when the number of nodes is 2000.

[0098] (2) Static scene 2

[0099] The size of the simulated static scene map is X=Y=500m, and all obstacles are static obstacles. The starting point is (50, 50), and the target point is (250, 450). Figure 6 As shown, it is a simulation map of static scene 2, point S represents the starting point, G represents the target point, the cyan line represents the tree structure, the green point represents the sampling point, and the red line represents the path planned by the algorithm proposed in the present invention when the number of nodes is 7000.

[0100] The above description is only a preferred embodiment of the present invention. For ordinary technicians in this field, according to the teachings of the present invention, designing different forms of motion planning algorithms does not require creative labor. Without departing from the principles and spirit of the present invention, all equal changes, modifications, substitutions and variations made within the scope of the patent application of the present invention should fall within the scope of the present invention.

Claims

1. A mobile robot path planning method based on homotopic space sampling FMT*, characterized by: The steps include: Step S1, map initialization: all known obstacles in the environment are discretized with a uniformly distributed set of points, and assuming that the number of obstacles is k, the set {C i },Right now Step S2, using an incremental construction method to establish a generalized Voronoi diagram, namely, GVG, which is composed of generalized Voronoi edges and vertices; Step S3, extracting feature nodes: obtaining the distance d from the nearest obstacle through the GVG establishment process, taking it as the radius R of the GVG vertex, comparing the positional relationship between the GVG vertices and the size of the radius R, extracting the GVG vertex with a larger radius as the feature node; Step S4: constructing a feature visibility graph: perform visibility detection on each pair of feature nodes, add edges between visible feature nodes, and these edges and visible feature nodes form a feature visibility graph T. F ; Step S5, find the initial heuristic path: set the initial position and target position of the robot, and find the initial heuristic path P in the feature visibility graph init , realize the homology between the initial heuristic path and the optimal path; Step S6, feature unit decomposition: according to the positional relationship between the state point and all feature nodes in the free space, the free space is decomposed into a number of feature units, each feature unit having only one feature node; Step S7, adjacency graph construction: determine whether two feature units are adjacent, add edges between corresponding feature nodes in adjacent feature units, and these edges and adjacent feature nodes form an adjacency graph T A ; Step S8, solving the heuristic path: starting from the initial position of the initial heuristic path, use the A* optimal graph search method in the adjacency graph to find the local path solution P to the next path point part ;Search for the local path solution to reach the next path point in sequence until reaching the target position, and concatenate all local path solutions into a heuristic path P connect ,The heuristic path is implemented by the adjacent feature nodes; Step S9, homotopy space calculation: construct the homotopy space S of the optimal path according to the heuristic path, use the homotopy space as the sampling space, and realize that the path in the sampling space is homotopic with the optimal path; Step S10, non-uniform sampling: set the total number of samples N, adjust the sampling coefficient according to the area of ​​each feature unit constituting the sampling space, perform random sampling in each feature unit according to the sampling coefficient, and obtain the sampling set V after taking the union unvisited ; Step S11: construct a sampling set V unvisited , Explore Collection V open 、Explored Set V closed , = the target point x of the robot path goal Put in V unvisited , the initial point x start Add to Exploration Set V open , the cost is initialized to 0, denoted as c(x start )=0, explored set Step S12: Find the exploration set V open The point with the minimum cost is taken as node Z, with Z as the center, r n Search sampling set V within the radius unvisited The nodes form a set X near , with the function Near(V unvisited ,Z,r n )express; Step S13: for each point x∈X near , with x as the center and radius r n Explore V open midpoint, forming set Y near , with the function Near(V open ,x,r n )express; Step S14, calculate all points y∈Y near The cost to get to point x, select the point y with the minimum cost min , expressed as Minimize the cost from each node to the child node in the tree to achieve the asymptotic optimality of the algorithm; Step S15: Determine y min Is there an obstacle with x? If not, move x from V unvisited Put in V open The cost is c(x)=c(y min )+Cost(y min ,x),y min is the parent node of x; Step S16: Exploration completed near Then, change Z from V open Put in V closed ; Step S17, determine whether Z is the target point, if so, return the path solution consisting of the points in the explored set, if not, return to step S12 to continue; Step S18: The robot receives the feasible path solution planned by the algorithm, and converts the path in the feasible path solution into the required linear velocity and angular velocity, controls the robot to move to the target position, and the path planning is completed.

2. The mobile robot path planning method based on homotopic space sampling FMT* according to claim 1 is characterized by: The specific implementation method of extracting characteristic nodes in step S3 is: Distance d i (x) represents the distance from point x to the i-th obstacle C i The distance is defined as: Define the direction from point x to c0, by the gradient Indicates that c0 represents the point x to C i The closest point to If there are many obstacles around point x, the distance from point x to the nearest obstacle is defined as This distance is taken as the obstacle-free radius R of point x; Put all GVG vertices into a GVG vertex set, which contains the coordinate information and radius R of each GVG vertex; Start the extraction operation: extract the GVG vertex with the largest radius from the GVG vertex set as the feature node, calculate the distance from other GVG vertices in the set to the GVG vertex, and if the distance is less than the radius R of the GVG vertex, remove the corresponding other GVG vertices from the GVG vertex set to complete an extraction operation; Repeat the above extraction operation until the GVG vertex set is an empty set. Assuming that m feature nodes are extracted, the feature node set {F j },Right now 3. The mobile robot path planning method based on homotopic space sampling FMT* according to claim 2 is characterized by: The step S5 of finding the initial heuristic path will use the A* optimal graph search method to find the current optimal solution in the feature visibility graph. According to the connectivity of the feature visibility graph, the current solution will be a homotopic solution with the global optimal path.

4. The mobile robot path planning method based on homotopic space sampling FMT* according to claim 2 is characterized in that: The specific implementation method of the feature unit decomposition in step S6 is: Represents point x to the jth feature node F j The characteristic distance is defined as Where R is the feature node F j The barrier-free radius; For a point x in free space, calculate the characteristic distances from point x to all characteristic nodes respectively, and define point x in the characteristic unit represented by the characteristic node with the smallest characteristic distance; All state points in the free space are divided into m feature units. The feature unit set is represented as {Cell j },Right now The intersection of two feature units is an empty set, and the union of all feature units is the entire free space. The feature units provide a concise representation of the topological relationship of environmental information.

5. The mobile robot path planning method based on homotopic space sampling FMT* according to claim 3 is characterized by: The specific implementation method of the homotopy space calculation in step S9 is: the heuristic path is composed of adjacent feature nodes, and the union of the feature units where these feature nodes are located is taken to form the homotopy space S of the optimal path.

6. The mobile robot path planning method based on homotopic space sampling FMT* according to claim 5, characterized in that: The specific implementation method of the non-uniform sampling of step S10 is: calculate the area A of each feature unit constituting the sampling space, that is, the number of grids contained in the feature unit, and record the area of ​​the jth feature unit as A j , the sum of the areas of all characteristic units constituting the sampling space is A s The ratio to the number of characteristic units is defined as the average area Then the sampling coefficient calculation method in the jth feature unit is: Where μ is the reference sampling coefficient, which is determined by the total number of samples N.

Images