A motion planning method for mobile robots based on obstacle-circling path optimization and biased sampling

Abstract

The present invention proposes a mobile robot motion planning method based on obstacle-surrounding path optimization and biased sampling, comprising the following steps: step S1, constructing a node set, an edge set, and a sampling point set not added to the search tree; step S2, constructing a queue and; step S3, if the processing of the batch of sampling points is completed, pruning is performed; step S4, if the processing of the batch of sampling points is completed, biased sampling is performed; step S5, if the optimal value of the queue is not greater than the optimal value of the queue, edge pre-expansion is performed; step S6, removing the edge with the minimum value; step S7, if the edge helps to improve the path quality, it will be added to the search tree; step S8, when the path meets the requirements, the path solution is returned, otherwise it returns to step S3 to continue; step S9, completing the path planning; compared with BIT*, the algorithm of the present invention can converge to the optimal solution faster and improve the exploration efficiency, and has advantages in incremental search path planning.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot path planning, and in particular to a mobile robot motion planning method based on obstacle-surrounding path optimization and biased sampling. Background Art

[0002] Mobile robots are currently widely used in diverse fields, including manufacturing, agriculture, logistics, and warehousing. Path planning is a key step in enabling mobile robots to perform tasks. Its primary function is to provide a continuous path from a starting point to a destination. This path ensures safety and collision avoidance while also satisfying certain task constraints. Commonly used planning methods include artificial potential field methods, genetic algorithms, graph search algorithms, and random sampling.

[0003] Sampling-based path planning methods have become a research hotspot because they can quickly find feasible paths in high-dimensional and complex environments. The Rapidly Exploring Random Tree (RRT) method proposed by LaValle et al. can effectively solve the path planning problem in high-dimensional space and quickly obtain feasible paths. However, because the sampling of the RRT method is random, the path obtained by this method is often non-optimal. Therefore, the RRT* method optimizes the nodes and their connections in the search tree by rewiring, and has the characteristics of asymptotic optimality. In order to further improve the efficiency of obtaining the optimal solution, the Informed RRT* method limits the sampling range to an ellipse based on the current path cost, and further narrows the sampling range each time the path is updated, thereby increasing the speed at which the path converges to the optimal solution. The FastMatching Tree (FMT*) method obtains a batch of samples in space and lazily expands the tree. This method does not require rewiring, and thus converges to the optimal solution faster.

[0004] The Batch Informed Trees (BIT*) method combines the advantages of the Informed RRT* method and the FMT* method to perform multi-batch sampling and expansion. BIT* sorts nodes and edges according to their estimated costs and performs a sorted search within an elliptical region, which avoids adding worthless nodes and edges to the search tree. Liu et al. and Strub et al. used greedy search strategies and asymmetric bidirectional search strategies, respectively, to accelerate the acquisition of BIT* initial solutions. However, the initial solutions obtained by these methods are not necessarily homotopic solutions to the optimal solution, and thus have limitations in improving subsequent paths. Summary of the Invention

[0005] In view of this, the purpose of the present invention is to provide a mobile robot motion planning method based on obstacle-surrounding path optimization and biased sampling, which overcomes the shortcomings of the BIT* method, can converge to the optimal solution more quickly and improve the exploration efficiency, and has advantages in incremental search path planning.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: a mobile robot motion planning method based on obstacle-circling path optimization and biased sampling, comprising the following steps:

[0007] Step S1: Construct a node set V, an edge set E, and a sampling point set V not added to the search tree. unconnected , the starting point x init Put in the node set V and edge set Target point x goal Put the sampling point set V unconnected In the search tree T = (V, E), the current path cost c i Initialized to infinity;

[0008] Step S2: Construct queue Q V and Q E , Q V Save the nodes z, Q in the search tree T in order E Save the edges (z, x) in the search tree T in a certain order, queue Q V =V and

[0009] Step S3: If all sampling points in the map have been processed, prune them to obtain a set V that still has the potential to improve path quality. reuse ;

[0010] Step S4: If all sampling points in the map have been processed, biased sampling is performed, a new batch of sampling points is added, and the sampling points are placed in the new sampling point set V. sampling and make V unconnected =V reuse ∪V sampling ;

[0011] Step S5: If the queue Q V The optimal value of the queue is not greater than Q E The optimal value of V Pre-expand the edges within the connection radius of the optimal node in ;

[0012] Step S6: From queue Q E Select and remove the edge with the smallest value (z min ,x min ),like and Among them, the function g T (z) represents the distance from node z to the starting point x init The path cost function Represents the estimated cost from node z to node x, function Represents the journey from node x to target point xgoal The estimated cost of the edge (z min ,x min ) to perform collision detection. If there is no collision, return the edge (z min ,x min )Cost c edge , otherwise c edge is infinite, if Then order and And return to step S3;

[0013] Step S7: If And g T (z min )+c edge <g T (x min ), then x min Add it to the search tree, and when the current path changes, optimize the current path and update the current path cost c i ;

[0014] Step S8: When the current path cost c i If the set requirements are met, the path solution is returned, otherwise it returns to step S3 to continue;

[0015] Step S9: The mobile robot receives the feasible path and converts the feasible path into a control instruction to control the mobile robot to move to the target position. The path planning is completed.

[0016] In a preferred embodiment: the queue Q in step S2 V and Q E The specific content of the sorting is: Q V The middle node z is from the current search tree T to the starting point x init The actual length from node z to target point x goal The sum of the estimated lengths of the functions is sorted from small to large, that is, by function Sort from small to large; Q E The middle edge (z,x) is from the node z in the current search tree T to the starting point x init The actual length of the node z, the estimated length from node z to node x, and the estimated length from node x to target point x goal The sum of the estimated lengths of the three is sorted from small to large, that is, by function Sort from small to large; let queue Q V and Q E The minimum value of the optimal value is recorded as the function QueueBest(Q V ) and QueueBest(Q E ).

[0017] In a preferred embodiment, the specific implementation of step S3 is: calculate the number of nodes z added to the search tree T to the starting point x. init The actual length and the target point x goal The sum of the estimated lengths of the paths is pruned and all calculated results are greater than the current path cost c. i Node; calculate all sampling points x that are not added to the search tree to the starting point x init The estimated length and to the target point x goal The sum of the estimated lengths of the paths is pruned and all calculated results are greater than the current path cost c. i Sampling points; save all the remaining points to the set V that still has the potential to improve the path quality reuse For the next batch of expansion.

[0018] In a preferred embodiment, the specific implementation of the biased sampling in step S4 is as follows: if the current path cost c i When it is not infinite, that is, the path has been found, the found path is represented as a set V consisting of path points path ={x init ,x1,x2,…,x goal}, calculate the heuristic value of all path points The maximum value is selected as the biased sampling parameter, denoted as F(x max ), starting point x init and the target point x goal For two foci, F(x max ) constructs an elliptical biased sampling region for the major axis length, which is a subset of the informed sampling region. Sampling in this region helps to accelerate path improvement and algorithm convergence;

[0019] Combine biased sampling with informed sampling to avoid the path falling into local optimality due to less information in the biased sampling area, and ensure the asymptotic optimality of the algorithm. Introduce a bias ratio α between 0 and 1. At each sampling, generate sampling points in the biased sampling area with a probability of α and save them in the new sampling point set V sampling Generate sampling points in the informed sampling area with a probability of 1-α and save them into the set V sampling In, until the set V sampling When the number of sampling points n reaches a given value, stop sampling and return the set V sampling ;

[0020] If the current path cost c i When it is still infinite, that is, no path is found, global uniform sampling is performed to obtain a set V with n sampling points. sampling .

[0021] In a preferred embodiment: the edge pre-expansion of step S5 is performed by V Take out and delete the first node z min , search set V unconnected In z min is the center of the circle, and r is the radius of all nodes within the range z min The set of neighboring points V near , calculate the set V near All nodes in value, all calculated values ​​less than c i The nodes and z min The edge (z min ,x) added to Q E middle.

[0022] In a preferred embodiment, the specific implementation content of the path optimization in step S7 is: define the path point set as V path ={x1,x2,…,x N}, U represents the discretization degree of the path connection, defined as U = round(10·d / r), where r is the current connection radius and d is the length of the longer edge between the two adjacent edges to be optimized;

[0023] If the edge (x i ,x i+2 ) No collision, then the path point x i+1 From the path point set V path Delete, then x i with x i+2 Direct connection;

[0024] If the edge (x i ,x i+2 ) collides, then from x i Start with is the step length to x i+1 Move circularly to obtain temporary node x temp , judge the edge (x i ,x temp ) and (x temp ,x i+2 ) whether a collision occurs, if no collision occurs, then x i+1 =x temp , the loop ends; if a collision occurs, temp Generate node x outward new , judge the edge (x i ,x new ) and (x new ,x i+2 ) whether a collision occurs, if no collision occurs, then x i+1 =x new , the loop ends;

[0025] Stop until all edges are optimized and obtain the optimized path set V path ,According to the triangle inequality, the sum of two sides is greater than the third side, ,the optimized path is shorter, and the path quality is improved.

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

[0027] (1) The present invention proposes a method for optimizing a path around obstacles. By processing the current path so that it circles around the obstacle, the path cost is reduced, the sampling range is reduced, and the speed of obtaining the optimal solution is accelerated.

[0028] (2) The speed of convergence to the optimal solution is the main performance of asymptotic optimal algorithms such as RRT* and BIT*. The present invention further narrows the range of the sampling ellipse through biased sampling, increases the possibility of obtaining sampling points that can improve the path quality, and allows the algorithm to converge to the optimal solution quickly. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Attachment Figure 1 It is a flow chart of a preferred embodiment of the present invention;

[0030] Attachment Figure 2 is a schematic diagram of biased sampling in a preferred embodiment of the present invention;

[0031] Attachment Figure 3 This is a schematic diagram of an obstacle-around path optimization according to a preferred embodiment of the present invention;

[0032] Attachment Figure 4 This is a schematic diagram of a simulation result of a static scene according to a preferred embodiment of the present invention;

[0033] Attachment Figure 5 This is a schematic diagram of simulation results of a static scene 2 according to a preferred embodiment of the present invention;

[0034] Attachment Figure 6 1 is a schematic diagram of simulation results of a static scene 3 according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

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

[0036] It should be noted that the following detailed descriptions are illustrative and intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which the present application belongs.

[0037] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application; as used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form, and it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or their combinations.

[0038] A mobile robot motion planning method based on obstacle-surrounding path optimization and biased sampling, Figure 1-6 , including the following steps:

[0039] Step S1: Construct a node set V, an edge set E, and a sampling point set V not added to the search tree. unconnected , the starting point x init Put in V, edge set Target point x goal Put V unconnected In the search tree T = (V, E), the current path cost c i Initialized to infinity;

[0040] Step S2: Construct queue Q V and Q E , Q V Save the nodes z, Q in the search tree in a certain order E Save the edges (z, x) in the search tree in a certain order, queue Q V =V and

[0041] Step S3: If the batch of sampling points is processed, prune the points to obtain the set V reuse ;

[0042] Step S4: If the processing of the batch of sampling points is completed, biased sampling is performed, a new batch of sampling points is added, and the sampling points are placed in the set V sampling and make V unconnected =V reuse ∪V sampling ;

[0043] Step S5: If the queue Q V The optimal value of the queue is not greater than Q E If the optimal value of is obtained, the edge is pre-expanded within the connection radius of the node itself;

[0044] Step S6: From queue Q E Select and remove the edge with the smallest value (z min ,x min ),like and Then the opposite side (zmin ,x min ) to perform collision detection. If there is no collision, return the edge (z min ,x min )Cost c edge , otherwise c edge is infinite, if Then order and And return to step S3;

[0045] Step S7: If And g T (z min )+c edge <g T (x min ), then x min Add to the search tree, and when the current path changes, optimize the current path and update c i ;

[0046] Step S8: When c i If the set requirements are met, the path solution is returned, otherwise it returns to step S3 to continue;

[0047] Step S9: The mobile robot receives the feasible path and converts the feasible path into a control instruction to control the mobile robot to move to the target position. The path planning is completed.

[0048] The queue Q of step S2 V and Q E The specific content of the sorting is: Q V The middle node z is from the current search tree T to the starting point x init The actual length from node z to target point x goal The sum of the estimated lengths of the functions is sorted from small to large, that is, by function Sort from small to large; Q E The middle edge (z,x) is from the node z in the current search tree T to the starting point x init The actual length of the node z, the estimated length from node z to node x, and the estimated length from node x to target point x goal The sum of the estimated lengths of the three is sorted from small to large, that is, by function Sort from small to large. Let queue Q V and Q E The minimum value of the optimal value is recorded as the function QueueBest(Q V ) and QueueBest(Q E ).

[0049] The specific implementation of step S3 is: calculate all nodes z added to the search tree T to the starting point x init The actual length and the target point xgoal The sum of the estimated lengths of the paths is pruned and all calculated results are greater than the current path cost c. i Node; calculate all sampling points x that are not added to the search tree to the starting point x init The estimated length and to the target point x goal The sum of the estimated lengths of the paths is pruned and all calculated results are greater than the current path cost c. i Sampling points. Save all the remaining points to the set V reuse In, it is used for the expansion of the next batch;

[0050] The planning method can retain only nodes with the potential to improve path quality through pruning, so as to reduce the maintenance cost of the tree and accelerate the convergence of the algorithm.

[0051] The specific implementation of the biased sampling in step S4 is: if c i When it is not infinite, that is, the path has been found, the found path is represented as a set V consisting of path points path ={x init ,x1,x2,…,x goal}, calculate the heuristic value of all path points The maximum value is selected as the biased sampling parameter, denoted as F(x max ), starting point x init and the target point x goal For two foci, F(x max ) constructs an elliptical biased sampling region for the major axis length, which is a subset of the informed sampling region. Sampling in this region helps to accelerate path improvement and algorithm convergence;

[0052] Combine biased sampling with informed sampling to avoid the path falling into local optimality due to less information in the biased sampling area, and ensure the asymptotic optimality of the algorithm. Introduce a bias ratio α between 0 and 1. At each sampling, generate sampling points in the biased sampling area with a probability of α and save them in the set V sampling middle;

[0053] Generate sampling points in the informed sampling area with a probability of 1-α and save them into the set V sampling In, until the set V sampling When the number of sampling points reaches the given value n, stop sampling and return the set V sampling ,like Figure 2 As shown;

[0054] If c i When it is still infinite, that is, no path is found, global uniform sampling is performed to obtain a set V of n sampling .

[0055] The edge pre-expansion of step S5 is performed by V Take out and delete the first node z min , search set V unconnected In z min is the center of the circle, r is all the nodes within the radius, these nodes form the set V near , calculate the set V near All nodes in value, all calculated values ​​less than c i The nodes and z min The edge (z min ,x) added to Q E middle;

[0056] The pre-expansion process can avoid adding edges that do not have the potential to improve the path into Q E Make judgments and improve computing efficiency.

[0057] The specific implementation content of the path optimization in step S7 is: define the path point set as V path ={x1,x2,…,x N}, U represents the discretization degree of the path connection, defined as U = round(10·d / r), where r is the current connection radius and d is the length of the longer edge between the two adjacent edges to be optimized;

[0058] If the edge (x i ,x i+2 ) No collision, then the path point x i+1 From the path point set V path Delete, then x i with x i+2 Direct connection;

[0059] If the edge (x i ,x i+2 ) collides, then from x i Start with is the step length to x i+1 Move circularly to obtain temporary node x temp , judge the edge (x i ,x temp ) and (x temp ,x i+2 ) whether a collision occurs, if no collision occurs, then x i+1 =x temp , the loop ends; if a collision occurs, temp Generate node x outward new , judge the edge (x i ,x new ) and (x new ,x i+2) whether a collision occurs, if no collision occurs, then x i+1 =x new , the loop ends;

[0060] Stop until all edges are optimized and obtain the optimized path set V path ,According to the triangle inequality, the sum of two sides is greater than the third side, the optimized path is shorter, and the path quality is improved, such as Figure 3 shown.

[0061] Example:

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

[0063]

[0064]

[0065]

[0066] Among them, Algorithm 1 is the main part of the algorithm, Algorithm 2 is the path optimization part, and Algorithm 3 is the biased sampling part.

[0067] The following is a detailed description of the embodiments of the present invention using specific experiments. The present invention is a motion planning algorithm with completeness and asymptotic optimality, and its effectiveness is mainly verified through simulation experiments. The specific experimental settings are as follows:

[0068] Simulation experiment:

[0069] The simulation experiments were carried out in MATLAB R2020b software.

[0070] (1) Static scene 1

[0071] The size of the simulated static scene map is X=Y=400m, and all obstacles are static obstacles. The starting point is (20, 200), the target point is (380, 200), and the number of sampling points per batch is 100. Figure 4 As shown in the figure, it is a simulation map of static scene 1. The equilateral triangle represents the starting point, the inverted triangle represents the target point, the circle is the sampling point, the dotted line is the informed sampling area, the light line represents the explored edge, and the dark line represents the path planned by the algorithm.

[0072] (2) Static scene 2

[0073] The size of the simulated static scene map is X=Y=400m, and all obstacles are static obstacles. The starting point is (20, 200), the target point is (380, 200), and the number of sampling points per batch is 100. Figure 5As shown in the figure, it is a simulation map of static scene 2. The equilateral triangle represents the starting point, the inverted triangle represents the target point, the circle is the sampling point, the dotted line is the informed sampling area, the light line represents the explored edge, and the dark line represents the path planned by the algorithm.

[0074] (3) Static scene three

[0075] The size of the simulated static scene map is X=Y=400m, and all obstacles are static obstacles. The starting point is (20, 200), the target point is (380, 200), and the number of sampling points per batch is 300. Figure 6 As shown, this is the simulation map of static scene three. The equilateral triangle represents the starting point, the inverted triangle represents the target point, the circle is the sampling point, the dotted line is the informed sampling area, the light line represents the explored edge, and the dark line represents the path planned by the algorithm.

[0076] The above 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 work. 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 motion planning method based on obstacle-circling path optimization and biased sampling, characterized by: The steps include: Step S1: Construct a node set V, an edge set E, and a sampling point set V not added to the search tree. unconnected , the starting point x init Put in the node set V and edge set Target point x goal Put the sampling point set V unconnected In the search tree T = (V, E), the current path cost c i Initialized to infinity; Step S2: Construct queue Q V and Q E , Q V Save the nodes z, Q in the search tree T in order E Save the edges (z, x) in the search tree T in a certain order, queue Q V =V and Step S3: If all sampling points in the map have been processed, prune them to obtain a set V that still has the potential to improve path quality. reuse ; Step S4: If all sampling points in the map have been processed, biased sampling is performed, a new batch of sampling points is added, and the sampling points are placed in the new sampling point set V. sampling and make V unconnected =V reuse ∪V sampling ; Step S5: If the queue Q V The optimal value of the queue is not greater than Q E The optimal value of V Pre-expand the edges within the connection radius of the optimal node in ; Step S6: From queue Q E Select and remove the edge with the smallest value (z min ,x min ),like and Among them, the function g T (z) represents the distance from node z to the starting point x init The path cost function Represents the estimated cost from node z to node x, function Represents the journey from node x to target point x goal The estimated cost of the edge (z min ,x min ) to perform collision detection. If there is no collision, return the edge (z min ,x min )Cost c edge , otherwise c edge is infinite, if Then order and And return to step S3; Step S7: If And g T (z min )+c edge <g T (x min ), then x min Add it to the search tree, and when the current path changes, optimize the current path and update the current path cost c i ; Step S8: When the current path cost c i If the set requirements are met, the path solution is returned, otherwise it returns to step S3 to continue; Step S9: The mobile robot receives the feasible path and converts the feasible path into a control instruction to control the mobile robot to move to the target position. The path planning is completed.

2. The mobile robot motion planning method based on obstacle-circling path optimization and biased sampling according to claim 1, characterized in that: The queue Q of step S2 V and Q E The specific content of the sorting is: Q V The middle node z is from the current search tree T to the starting point x init The actual length from node z to target point x goal The sum of the estimated lengths of the functions is sorted from small to large, that is, by function Sort from small to large; Q E The middle edge (z,x) is from the node z in the current search tree T to the starting point x init The actual length of the node z, the estimated length from node z to node x, and the estimated length from node x to target point x goal The sum of the estimated lengths of the three is sorted from small to large, that is, by function Sort from small to large; let queue Q V and Q E The minimum value of the optimal value is recorded as the function QueueBest(Q V ) and QueueBest(Q E ).

3. The method for mobile robot motion planning based on obstacle-circling path optimization and biased sampling according to claim 1, characterized in that: The specific implementation of step S3 is: calculate all nodes z added to the search tree T to the starting point x init The actual length and the target point x goal The sum of the estimated lengths of the paths is pruned and all calculated results are greater than the current path cost c. i Node; calculate all sampling points x that are not added to the search tree to the starting point x init The estimated length and to the target point x goal The sum of the estimated lengths of the paths is pruned and all calculated results are greater than the current path cost c. i Sampling points; save all the remaining points to the set V that still has the potential to improve the path quality reuse For the next batch of expansion.

4. The method for mobile robot motion planning based on obstacle-circling path optimization and biased sampling according to claim 1, characterized in that: The specific implementation of the biased sampling in step S4 is: if the current path cost c i When it is not infinite, that is, the path has been found, the found path is represented as a set V consisting of path points path ={x init ,x1,x2,…,x goal }, calculate the heuristic value of all path points The maximum value is selected as the biased sampling parameter, denoted as F(x max ), starting point x init and the target point x goal For two foci, F(x max ) constructs an elliptical biased sampling region for the major axis length, which is a subset of the informed sampling region. Sampling in this region helps to accelerate path improvement and algorithm convergence; Combine biased sampling with informed sampling to avoid the path falling into local optimality due to less information in the biased sampling area, and ensure the asymptotic optimality of the algorithm. Introduce a bias ratio α between 0 and 1. At each sampling, generate sampling points in the biased sampling area with a probability of α and save them in the new sampling point set V sampling middle; Generate sampling points in the informed sampling area with a probability of 1-α and save them into the set V sampling In, until the set V sampling When the number of sampling points n reaches a given value, stop sampling and return the set V sampling ; If the current path cost c i When it is still infinite, that is, no path is found, global uniform sampling is performed to obtain a set V with n sampling points. sampling .

5. The method for mobile robot motion planning based on obstacle-circling path optimization and biased sampling according to claim 2, characterized in that: The edge pre-expansion of step S5 is performed by V Take out and delete the first node z min , search set V unconnected In z min is the center of the circle, and r is the radius of all nodes within the range z min The set of neighboring points V near , calculate the set V near All nodes in value, all calculated values ​​less than c i The nodes and z min The edge (z min ,x) added to Q E middle.

6. The method for mobile robot motion planning based on obstacle-circling path optimization and biased sampling according to claim 1, characterized in that: The specific implementation content of the path optimization in step S7 is: define the path point set as V path ={x1,x2,…,x N }, U represents the discretization degree of the path connection, defined as U = round(10·d / r), where r is the current connection radius and d is the length of the longer edge between the two adjacent edges to be optimized; If the edge (x i ,x i+2 ) No collision, then the path point x i+1 From the path point set V path Delete, then x i with x i+2 Direct connection; If the edge (x i ,x i+2 ) collides, then from x i Start with is the step length to x i+1 Move circularly to obtain temporary node x temp , judge the edge (x i ,x temp ) and (x temp ,x i+2 ) whether a collision occurs, if no collision occurs, then x i+1 =x temp , the loop ends; if a collision occurs, temp Generate node x outward new , judge the edge (x i ,x new ) and (x new ,x i+2 ) whether a collision occurs, if no collision occurs, then x i+1 =x new , the loop ends; Stop until all edges are optimized and obtain the optimized path set V path ,According to the triangle inequality, the sum of two sides is greater than the third side, ,the optimized path is shorter, and the path quality is improved.

Images