Mobile robot path planning method based on GRRT*- DWA algorithm

Abstract

The invention discloses a mobile robot path planning method based on a GRRT *-DWA algorithm, and relates to the technical field of path planning, and the method comprises the steps: constructing the GRRT *-DWA algorithm: generating the GRRT * algorithm; adding a sub-evaluation function into the DWA algorithm to obtain an improved DWA algorithm; the GRRT * algorithm and the improved DWA algorithm are fused to obtain a GRRT *-DWA algorithm; path planning is carried out on the mobile robot through a GRRT *-DWA algorithm; global path planning is carried out through the GRRT * algorithm; based on global path planning, optimal local planning is carried out through an improved DWA algorithm; and completing path planning. According to the method, the path optimization degree, the planning efficiency and the effective rate are high, the path cost is low, and the method also has relatively high robustness and effectiveness in a complex environment.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular, to a path planning method for a mobile robot. Background Art

[0002] With the continuous development of robot technology, robots already have some capabilities that can replace humans and are being applied more and more widely. As one of the focuses of robot research, mobile robots have stood out both at home and abroad. Its application scope covers hospitals, railway stations, warehouses, shopping markets, etc. Therefore, researching the optimal path planning method for mobile robots has important theoretical value and practical significance. Some achievements have been made in path planning research from the beginning to the present, but relatively speaking, these are still far from enough. In recent years, domestic and foreign research scholars have conducted more in-depth research on path planning algorithms. According to the degree of grasp of environmental information, path planning can be divided into global path planning based on prior complete information and local path planning based on sensor information.

[0003] Among them, from the perspective of whether the obstacle information obtained is static or dynamic, it is divided into global path planning and local path planning. Global path planning belongs to static planning, and local path planning belongs to dynamic planning. Global path planning requires mastering all environmental information and performing path planning based on all information in the environmental map. According to the classification of path planning algorithms, the general algorithms for global path optimal planning can be divided into three categories. Graph search algorithms, such as Dijkstra, A*, D*, and their corresponding extensions. Path planning algorithms based on random sampling, which randomly generate sampling points in the state space and then gradually expand the tree structure to connect the starting node and the target node to find a feasible path, including Rapidly-exploring Random Tree (RRT) and its improved versions. Bio-inspired algorithms, which skip the process of constructing a complex environmental model and use random methods to search for the optimal path, including Ant Colony Optimization (ACO) algorithm, Particle Swarm Optimization (PSO) algorithm, and Differential Evolution (DE) algorithm, etc. Local path planning only needs to collect environmental information in real time by sensors, understand the environmental map information, and then determine the position in the map and the local obstacle distribution, so as to select the optimal path from the current node to a certain sub-goal node. Local path planning algorithms are mainly divided into deterministic algorithms and probabilistic algorithms. Deterministic algorithms usually have good real-time performance and stability, but may be affected by local optimal solutions, mainly including Artificial Potential Field Method (APF), fuzzy logic algorithm, Dynamic Window Approach (DWA), spline curve method, Bezier curve method, etc. Probabilistic algorithms have strong global search capabilities, but have a large amount of computation and relatively poor real-time performance, mainly including Rapidly-exploring Random Tree (RRT), Probabilistic RoadMap (PRM), and random sampling algorithms.

[0004] With the development of the industrial society, traditional mobile robot path planning methods have encountered challenges, and more and more scholars have studied mobile robot path planning methods. Wang Wenjuan et al. proposed an AGV path planning method based on the Rapidly-exploring Random Tree-Ant Colony Optimization (RRT*-ACO). By combining the fast search mechanism of the RRT* algorithm and the positive feedback advantage of the ant colony algorithm, as well as introducing bidirectional search, heuristic dynamic sampling, and dynamic step size strategy, the efficiency and robustness of path planning were improved. Yin Xiong et al. proposed a mobile robot path planning method based on the improved RRT and TEB algorithms. By using the improved RRT algorithm to generate the global optimal path, and through methods such as adaptive sampling function, reducing path redundancy, and constraining the steering angle, the path planning performance was enhanced. At the same time, the TEB algorithm was combined to optimize the trajectory to conform to the kinematics of the robot. Wang Qian et al. proposed a distributed multi-mobile robot path planning and obstacle avoidance algorithm based on ACO-DWA. The algorithm combines Ant Colony Optimization (ACO) and Dynamic Window Approach (DWA), coordinates the multi-robot system through a priority strategy, and enhances the search ability of ACO by using a double-population heuristic function and an ordered pheromone update strategy. Wu Bin et al. proposed a dynamic path planning algorithm for Forklift Automatic Guided Vehicle (FAGV) based on the improved A* and improved Dynamic Window Approach (DWA). The method is to combine the improved A algorithm and the improved DWA algorithm. The improved A algorithm is used to plan a global optimal path more suitable for FAGV, and the improved DWA algorithm ensures that the local path of FAGV is closer to the global path.

[0005] However, today's path planning models have problems such as long calculation time, inability to select the optimal path solution in complex environments, and poor anti-interference ability. For example, in 2010, the RRT* algorithm proposed by S. Karaman and E. Frazzoli et al. can only consider the optimal path planning in a static environment and has relatively low efficiency. In the Dynamic Window Approach (DWA) algorithm, the calculation of the velocity space is beneficial for effective motion control, but it is easy to fall into local minima. Summary of the Invention

[0006] Aiming at the technical problems that mobile robots cannot adapt to and judge unknown environments and dynamic obstacles when performing purposeful navigation and moving in a dynamic and complex environment, and are prone to falling into local optima when planning local paths, the present invention proposes a path planning method for mobile robots based on the GRRT*-DWA algorithm. The sampling efficiency of the random tree is improved through the feedback deviation sampling strategy in the GRRT* algorithm, the problem of low path optimization efficiency is solved through the extended node filtering strategy, the adaptive step size strategy is used to replace the original fixed step size method in the RRT* algorithm to improve the path finding speed of the optimal path planning; the optimal path planning is realized by introducing the greedy algorithm; the DWA algorithm is improved by introducing a sub-evaluation function, and the improved DWA algorithm is used to avoid dynamic obstacles in real time while not deviating from the global optimal path. Furthermore, a full-course navigation path can be planned in a short time, dynamic obstacles can be accurately avoided in real time, and at the same time, it has real-time performance, accuracy and efficiency.

[0007] In order to achieve the above object, the technical solution of the present invention is realized as follows:

[0008] A path planning method for a mobile robot based on the GRRT*-DWA algorithm, comprising the following steps:

[0009] Step 1: Construct the GRRT*-DWA algorithm: Generate the GRRT* algorithm; add a sub-evaluation function to the DWA algorithm to obtain an improved DWA algorithm; fuse the GRRT* algorithm with the improved DWA algorithm to obtain the GRRT*-DWA algorithm;

[0010] Step 2: Perform path planning for the mobile robot through the GRRT*-DWA algorithm: Perform global path planning through the GRRT* algorithm; based on the global path planning, perform optimal local planning through the improved DWA algorithm; complete the path planning.

[0011] Further, the method for generating the GRRT* algorithm is: First introduce the feedback deviation sampling strategy, the extended node filtering strategy and the adaptive step size strategy into the RRT* algorithm, and then fuse the greedy algorithm.

[0012] Further, the method for performing global path planning through the GRRT* algorithm is: Perform node sampling according to the feedback deviation sampling strategy, perform node filtering according to the extended node filtering strategy, and search for an initial global path according to the adaptive step size strategy; use the path nodes of the initial global path as a reference, and use the greedy algorithm to realize the optimal path planning and output the global optimal path planning;

[0013] The method for optimal local planning through the improved DWA algorithm is as follows: Based on the global optimal path planning, the path nodes of the global optimal path planning are extracted as the path traction points of the improved DWA algorithm, and the optimized path is constrained not to deviate from the total global path. The optimal local planning trajectory is selected based on the evaluation function with the added sub-evaluation function, and the optimal local path is output.

[0014] Further, the method for node sampling according to the feedback deviation sampling strategy is as follows:

[0015] (1), Set the starting point as X init , generate the nearby nodes X near of the starting point in the direction of the end point, and expand the child nodes X near from the nearby nodes X new , and generate a random point X rand ;

[0016] (2), If the random point X rand does not trigger collision detection and is located inside the obstacle, it is the first new random point X rand_1 . The child node X new obtains the angle information θ of the first new random point X rand_1 , and reselects the expansion area, discarding the left and right expansions on the angle information θ;

[0017] (3), Repeat the process described in step (2) until the expansion of the random point X rand is no longer located inside the obstacle, which is the second new random point X rand_2 . Reselect the random point X new from the child node X rand .

[0018] Further, the expression of the angle information θ is:

[0019]

[0020] Among them, the coordinates of the child node X new are (x new , y new ), and the coordinates of the random point x rand are (x rand , y rand );

[0021] When the starting point searches for a random point and encounters an obstacle, a random offset value will be generated. The expression of the offset value is:

[0022] Β = Rand(-0.1, 0.01)

[0023] Among them, B is the offset value, and Rand is the random number function.

[0024] Furthermore, the method for performing node filtering according to the extended node filtering strategy is:

[0025] Assume that the node to be expanded is X wait , first determine the node X to be expanded wait Whether it can pass the collision test. If not, the node to be expanded X that fails the collision test will be discarded. wait ;

[0026] If it passes, calculate the node X to be expanded that passes the collision detection based on the Euclidean distance. wait The cost function is:

[0027] f(X wait )=d(X wait ,X init )+d(W wait ,X goal )

[0028] Among them, f(X wait ) is the node X to be expanded that passes the collision detection wait The cost function is d, which is the function for calculating the Euclidean distance between two points, X goal is the end point of the path;

[0029] If the current node to be expanded is X wait The cost function f(X wait ) is less than the path cost of the current path, then the current node to be expanded X wait Add to the tree node; otherwise, filter and resample directly.

[0030] Furthermore, the method for searching a path according to the adaptive step size strategy is:

[0031] At the beginning of the GRRT* algorithm, the initial step size is set according to the size of the map environment used, and the minimum step size is set to half of the initial step size;

[0032] At the beginning, the initial path is searched using the initial step size;

[0033] As the number of iterations increases, the global adaptive step size factor is used to gradually reduce the initial step size to the minimum step size. Each time the initial step size is reduced, the reduced initial step size is used to search for the path.

[0034] Furthermore, the method of using the path nodes of the initial global path as a reference, using a greedy algorithm to implement the optimal path planning, and outputting the global optimal path planning is:

[0035] ①From the end point X goal Start by setting the distance from the end point of the path to X goal The closest previous path point is Xn , connect the path end point X goal to the path point X n . If the connection line between the path end point X goal and the path point X n does not collide with the obstacle, then remove the path point X n , connect the path end point X goal to the previous path point X n nearest to the path point X n-1 ;

[0036] If the connection line between the path end point and the path point X n-1 collides with the obstacle, then retain the path point X n , and make the retained path point X n become the new parent node;

[0037] ② Repeat the process described in step ① until the path end point X goal or the new parent node is connected to the starting point X init to end.

[0038] Furthermore, the expression of the global adaptive step size factor is:

[0039]

[0040] step = k × step0

[0041] where a and b are both parameters for controlling step size adjustment, t and T are the current iteration number and the maximum iteration number respectively, step and step0 are the current step size and the initial step size respectively, and exp is the exponential function.

[0042] Furthermore, the expression of the evaluation function with the sub - evaluation function added is:

[0043] G(v, ω) = α * normal(heading(v, ω)) + β * normal(obs_dist(v, ω)) + γ * normal(velocity(v, ω)) + δ · normal(astar(v, ω))

[0044] Among them, normal() represents the normalization function; v is the linear velocity of the robot, and ω is the angular velocity of the robot; heading(v, ω) is the target direction score obtained by measuring the angle between the current moment and the next arrival moment of the robot and the end point, and α is the weight of the target direction score; obs_dist(v, ω) is the obstacle distance score obtained by measuring the distance between the robot and the obstacle, and β is the weight of the obstacle distance score; velocity(v, ω) is the velocity score obtained by measuring the difference between the velocity of the robot and the optimal velocity, and γ is the weight of the velocity score; δ is the weight of the sub - evaluation function astar(v, ω).

[0045] The expression of the sub - evaluation function astar(v, ω) is as follows:

[0046] astar(v, ω) = h1 + h2

[0047] Among them, h1 represents the Euclidean distance between the current position of the robot and the end point of the predicted trajectory, and h2 represents the Euclidean distance between the end point of the predicted trajectory and the target point.

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

[0049] The present invention combines the GRRT* algorithm with the improved DWA algorithm. Guided by the globally optimal path planned by the GRRT* algorithm search, the path points are extracted as the guiding points for local path planning, and the optimal local planning is carried out through the improved DWA algorithm, enabling the mobile robot to perform local real - time obstacle avoidance without deviating from the global path even in a complex environment. The path optimization degree, planning efficiency, and effectiveness of the present invention are superior to traditional methods and other improved algorithms, and the path cost is small, and it also has high robustness and effectiveness. Description of the Drawings

[0050] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0051] Figure 1 It is the flowchart of the present invention.

[0052] Figure 2 It is the optimized path diagram of the RRT* algorithm.

[0053] Figure 3 It is the flowchart of the GRRT* algorithm of the present invention.

[0054] Figure 4 This is the schematic diagram of the feedback deviation sampling strategy of the present invention.

[0055] Figure 5 This is the effect diagram of optimizing the initially searched global path by using the greedy algorithm in an embodiment of the present invention.

[0056] Figure 6 This is the schematic diagram of different simulation environments in an embodiment of the present invention; among them, Figure 6 -(a) is the schematic diagram of a simple environment; Figure 6 -(b) is the schematic diagram of a complex environment; Figure 6 -(c) is the schematic diagram of a complex dynamic environment.

[0057] Figure 7 This is the comparison diagram of simulation results in a simple environment in an embodiment of the present invention.

[0058] Figure 8 This is the comparison diagram of simulation results in a complex environment in an embodiment of the present invention.

[0059] Figure 9 This is the comparison diagram of simulation results in a sub-complex dynamic environment in an embodiment of the present invention. Detailed implementation manners

[0060] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0061] A mobile robot path planning method based on the GRRT*-DWA algorithm, as Figure 1 shown, includes the following steps:

[0062] Step 1. Construct the GRRT*-DWA algorithm: First, introduce the feedback deviation sampling strategy, the extended node filtering strategy, and the adaptive step size strategy into the RRT* algorithm, and then fuse the greedy algorithm to generate the GRRT* algorithm; add a sub-evaluation function to the DWA algorithm to obtain an improved DWA algorithm; fuse the GRRT* algorithm with the improved DWA algorithm to obtain the GRRT*-DWA algorithm.

[0063] The Rapidly-Exploring Random Tree Star (RRT*) is a global path planning algorithm, representing an enhancement of the Rapidly-Exploring Random Tree (RRT). The core method consists of several steps: The RRT* algorithm starts from the starting point and gradually constructs an exploration tree through random sampling and node expansion. Each node represents a state of the robot in the environment, and each edge represents the movement of the robot from one state to another. In each iteration, first, the RRT* algorithm selects a target point in the state space through random sampling to ensure uniform exploration of the tree. Second, the RRT* algorithm finds the nearest node in the current tree as the expansion node to ensure that the search is expanded based on the existing path information. By moving from the expansion node towards the target point, a new node is generated. The generation of the new node requires collision detection to ensure the feasibility of the path. Different from the RRT algorithm, after the generation of the new node, the RRT* algorithm will consider whether to optimize the path by reconnecting the nodes on the tree. By calculating the cost passing through the new node, the RRT* algorithm can dynamically optimize the structure of the tree during the search process, making the path more optimized and effective. When the target point is added to the tree, the RRT* algorithm stops searching and obtains an optimized path, as Figure 2 shown.

[0064] The RRT* algorithm uses a cost function to select the node with the minimum cost in the neighborhood as the parent node to improve the search efficiency of the algorithm. In addition, after each iteration, the RRT* algorithm will reconnect the nodes on the existing random tree. After obtaining the initial path, the RRT* algorithm will iterate to the maximum number of times, continuously optimizing the path cost value between the starting and ending points to achieve the purpose of asymptotically optimal solution.

[0065] However, the RRT* algorithm has deficiencies such as easy repeated aggregation of random sampling points, low search efficiency, long algorithm search time, strong randomness, and many redundant path points, so optimization is needed.

[0066] The RRT* algorithm explores the configuration space based on the method of random sampling. Such a method not only has low search efficiency in complex or dense obstacle areas, but also the quality of the generated path may be poor. To address the above problems, the present invention adopts a feedback bias sampling strategy to improve the RRT* algorithm. The feedback bias sampling strategy combines the ideas of exploration and exploitation and adopts a more intelligent sampling method to improve the efficiency of path planning.

[0067] The RRT* algorithm will generate a large number of redundant nodes during the search, resulting in the path not being optimal and shortest. To improve the efficiency of path optimization of the RRT* algorithm, the present invention introduces an expansion node filtering strategy into the RRT* algorithm.

[0068] The step size directly affects the path quality and search efficiency of the RRT* algorithm during search. The step size used by the RRT* algorithm is set in advance before the algorithm runs, without considering the influence of the environment on the step size, and the RRT* algorithm uses the same initial set step size every time a new node is expanded. When facing an area with many obstacles, if the initially set step size is large, it may cause many newly added nodes to fail the collision detection, resulting in a waste of the number of iterations. On the contrary, when facing an area with few obstacles, if the initially set step size is small, it will cause insufficient search ability and also slow down the iteration speed. Moreover, during the operation of the RRT* algorithm, it does not use the obstacle information near the nodes to adjust the step size, and it is prone to the situation where it cannot expand in a narrow area. Therefore, the present invention introduces an adaptive step size strategy into the RRT* algorithm.

[0069] Although the path planned by the RRT* algorithm after introducing the feedback bias sampling strategy, the expanded node filtering strategy, and the adaptive step size strategy is relatively optimal, there are still some redundant nodes, resulting in a relatively tortuous path. And the nodes that make up the optimal path are usually close to the obstacles. The fewer the generated nodes, the more quickly the optimal path can be planned. While satisfying obstacle avoidance, removing the redundant nodes is beneficial to further reducing the path cost. The present invention uses a greedy algorithm for path shortening.

[0070] In summary, the present invention improves the process of generating new nodes in the RRT* algorithm. By introducing the feedback bias sampling strategy, the expanded node filtering strategy, and the adaptive step size strategy into the RRT* algorithm, the search efficiency is improved, and thus a sub-optimal path can be generated in a short time. Then, a greedy algorithm is introduced to optimize and simplify the global path through the greedy algorithm to obtain the optimal solution of the global path. The flowchart of the generated GRRT* algorithm is as Figure 3 shown.

[0071] When the robot is in a dynamic and complex environment, local path planning algorithms are often needed to optimize the path planning and perform real-time dynamic obstacle avoidance through local path planning algorithms. There are many local path planning algorithms, and the DWA algorithm is one of the most common algorithms. The basic principle of the DWA algorithm abandons the problem of only considering the position of the robot in previous other algorithms. Instead, it takes the linear velocity and angular velocity of the robot as the initial input information for sampling. By calculating the position trajectory of the robot within a certain future time, all the trajectories allowed by the hardware conditions of the robot are obtained. Then, a corresponding evaluation function is formulated to evaluate the optimal information in these trajectories, and finally, the optimal trajectory is selected as the next trajectory point of the robot.

[0072] However, the evaluation function of the DWA algorithm only considers real-time information and lacks the guidance of global information. It only takes into account the obstacles and target positions in the current environment, which easily makes the robot fall into a local optimum rather than the global optimum path, thus deteriorating the global optimality of the path. To solve this problem and enable the robot to consider the guidance of global information, the present invention improves the evaluation function of the DWA algorithm, adds a sub-evaluation function to the DWA algorithm to obtain an improved DWA algorithm, and through the improved DWA algorithm, while not deviating from the global path, performs optimal local planning.

[0073] After improving the DWA algorithm, the improved DWA algorithm is fused with the GRRT* algorithm to create a new hybrid path planning algorithm called the GRRT*-DWA algorithm. After performing global path planning through the GRRT* algorithm, the global optimum path in the static environment is obtained, and the path points in the global path are extracted as the heuristic traction points of the improved DWA algorithm. When the mobile robot is in a dynamic environment, it will perform local path planning based on the global path planned by the GRRT* algorithm and does not deviate from the global path in the static environment area, thereby enhancing the algorithm's ability to balance global and local information and planning the optimal path that meets the traveling conditions of the mobile robot.

[0074] Step 2: Perform path planning for the mobile robot through the GRRT*-DWA algorithm:

[0075] Perform global path planning through the GRRT* algorithm: Sample nodes according to the feedback deviation sampling strategy to improve the sampling efficiency of the random tree; Filter nodes according to the expanded node filtering strategy to improve the path optimization efficiency; Search for the initial global path according to the adaptive step size strategy to improve the pathfinding speed of the optimal path planning;

[0076] Taking the path nodes of the initial global path as a reference, use the greedy algorithm to achieve optimal path planning and output the global optimum path planning.

[0077] Perform optimal local planning through the improved DWA algorithm: Based on the global optimum path planning, extract the path nodes of the global optimum path planning as the path traction points of the improved DWA algorithm, so that when the improved DWA algorithm performs local path optimization, the optimized path is constrained not to deviate from the total global path, realizing dynamic real-time obstacle avoidance while not deviating from the global optimum path. Select the optimal local planning trajectory based on the evaluation function with the added sub-evaluation function and output the optimal local path.

[0078] Specifically, the method of sampling nodes according to the feedback deviation sampling strategy is:

[0079] The feedback-biased sampling strategy is implemented as follows: In the areas already explored by the tree, sampling is preferred to more quickly find a path that may approximate the optimal one; there is a certain probability of exploring new areas not covered by the tree to discover potentially better paths or avoid potential obstacles. Generally speaking, it dynamically adjusts the sampling probability according to feedback information, preferentially selecting areas close to the known optimal path for sampling to optimize the path and improve efficiency.

[0080] By introducing the feedback-biased sampling strategy, the random tree can accurately determine the angle information when expanding to an obstacle, thus affecting the expansion direction of subsequent nodes. The feedback-biased sampling strategy dynamically adjusts the sampling probability through real-time feedback, significantly improving the overall performance of the GRRT* algorithm. During the execution of the GRRT* algorithm, feedback information on the effectiveness, quality, and density of sampling points is systematically collected, shifting the sampling focus to unexplored areas or near existing paths, thereby effectively exploring new paths. If the sampling point density in a certain area is high but no optimized path is found, the sampling frequency in that area will be reduced to reduce unnecessary exploration.

[0081] For the specific principle, see Figure 4 :

[0082] (1). Set the starting point as X init , generate nearby nodes X of the starting point in the direction of the end point near , nearby nodes X near expand to generate child nodes x new , and generate a random point X rand .

[0083] (2). If the random point X rand does not trigger collision detection and is within the obstacle, it is the first new random point X rand_1 , the child node x new obtains the angle information θ of the first new random point X rand_1 , and reselects the expansion area, discarding the left and right expansions in the angle information θ.

[0084] (3). Repeat the process described in step (2) until the expansion of the random point X rand is no longer within the obstacle, then it is the second new random point X rand_2 , reselect the random point X from the child node X new node rand .

[0085] Specifically, the expression for calculating the angle information θ is:

[0086]

[0087] Among them, the coordinates of the child node x new are (x new, y new ), the coordinates of the random point X rand are (x rand , y rand ).

[0088] When the child node X new encounters an obstacle during the search for a random point, a random offset value will be generated. To ensure the search area, the offset value should not be too large, and the expression for the offset value is:

[0089] Β = Rand(-0.1, 0.01)

[0090] where B is the offset value, Rand is the offset function, and the offset value should not be too large.

[0091] Compared with random sampling, feedback-biased sampling adjusts the sampling probability through feedback signals, making the GRRT* algorithm tend to select samples that generate high feedback, thereby improving the sample utilization efficiency; and reducing the number of feedback samples to avoid wasting computing resources on invalid or sub-optimal samples; at the same time, it can avoid the expansion of the random tree in certain obstacle areas, be able to dynamically adapt to uncertain or changing environments, and continuously optimize the path, which not only reduces the time to obtain the path but also reduces the number of expanded nodes; at different training stages, the GRRT* algorithm dynamically adjusts the sampling probability through feedback signals, thereby enhancing adaptability, accelerating overall convergence, and improving stability and adaptability in complex or changing environments.

[0092] Specifically, the method of filtering nodes according to the expanded node filtering strategy is as follows:

[0093] Assume the node to be expanded is X wait , first judge whether the node to be expanded X wait can pass the collision detection. The collision detection is to connect the node to be expanded x wait with the previous node and judge whether the path formed by the connection intersects with the obstacle. If it fails, discard the node to be expanded x wait that fails the collision detection; if it passes, calculate the cost function f(X wait ) of the node to be expanded X wait that passes the collision detection according to the Euclidean distance. The expression is as follows:

[0094] f(X wait ) = d(X wait , X init ) + d(X wait , X goal )

[0095] where d is the function for calculating the Euclidean distance between two points, and X goal is the end point of the path.

[0096] If the current node to be expanded is X wait The cost function f(X wait ) is less than the path cost of the current path, then the current node to be expanded X wait Add to the tree node; otherwise, filter and resample directly.

[0097] The extended node filtering strategy can avoid adding too many useless nodes in the tree to optimize the path search efficiency.

[0098] Specifically, the method for searching the initial global path according to the adaptive step size strategy is:

[0099] In view of the problem of fixed step size in the RRT* algorithm, the present invention proposes a global adaptive step size factor k. Considering the problem of computational efficiency, the optimal path convergence is achieved by changing the step size factor k according to the change of time. At the beginning of the GRRT* algorithm, the initial step size is set according to the size of the map environment used, and the minimum step size is set to half of the initial step size to prevent a large number of redundant calculations;

[0100] At the beginning, use the initial step size to quickly search for the initial path;

[0101] As the number of iterations increases, the global adaptive step size factor is used to gradually reduce the initial step size to the minimum step size. Each time the initial step size is reduced, the reduced initial step size is used to search for the path to perform a detailed path search and find a more optimized path.

[0102] The minimum step size is set to half of the initial step size to limit the amount of calculation of the algorithm and avoid falling into the local optimal solution in the later stage of path search, which reduces the search efficiency.

[0103] The expression of the global adaptive step size factor is:

[0104]

[0105] step=k×step0

[0106] Among them, a and b are parameters for controlling the step size adjustment. Parameter a controls the speed of step size reduction. When the value of parameter a is large, the step size decreases faster, and when the value of parameter a is small, the step size decreases slower. Parameter b controls the amplitude of step size reduction. The larger the b value, the faster the global adaptive step size factor decays, and the more drastic the step size reduction is. Conversely, it is slow. The values ​​of parameter a and parameter b are both positive numbers. t and T are the current number of iterations and the maximum number of iterations, respectively. step and step0 are the current step size and the initial step size, respectively. exp is an exponential function.

[0107] Specifically, taking the path nodes of the initial global path as a reference, the method for implementing optimal path planning using the greedy algorithm and outputting the global optimal path planning is as follows:

[0108] ① Starting from the path end point X goal set the previous path point closest to the path end point X goal as X n Connect the path end point X goal to the path point X n If the connection line between the path end point X goal and the path point X n does not collide with obstacles, then remove the path point X n Connect the path end point X goal to the previous path point X n closest to the path point X n-1 ;

[0109] If the connection line between the path end point and the path point X n-1 collides with obstacles, then retain the path point X n and let the retained path point X n become the new parent node.

[0110] ② Repeat the process described in step ① until the path end point X goal or the new parent node is connected to the starting point X init ends, as shown in Figure 5 .

[0111] Figure 5 In, X n , X n-1 , X n-2 and X n-4 are the nodes removed by the greedy algorithm, that is, the path of the global plan is optimized from X init -X n-4 -X n-3 -X n-2 -X n-1 -X n -X goal to X init -X n-3 -X goal , greatly shortening the path distance to achieve less energy consumption of the mobile robot.

[0112] The GRRT* algorithm proposed by the present invention not only significantly reduces the number of nodes in the path, making the path more concise and intuitive, but also makes the path smoother and shorter by reducing unnecessary turns and nodes, reducing the driving time and energy consumption of the mobile robot. When the robot is in a static environment, optimal path planning can be performed through the GRRT* algorithm.

[0113] Then, the DWA algorithm is improved according to the heuristic function in the A* algorithm. Specifically, the expression of the evaluation function G(v, ω) that adds the sub-evaluation function astar(v, ω) is as follows:

[0114] G(v, ω) = α * normal(heading(v, ω)) + β * normal(obs_dist(v, ω)) + γ * normal(velocity(v, ω)) + δ * normal(astar(v, ω))

[0115] where normal() represents the normalization function, which enables mutual comparison and weighting among various evaluation indicators, thereby optimizing the motion control of the robot; v is the linear velocity of the robot, ω is the angular velocity of the robot; heading(v, ω) is the target direction score obtained by measuring the angle between the current moment of the robot and the next arrival moment and the end point, and α is the weight of the target direction score; obs_dist(v, ω) is the obstacle distance score obtained by measuring the distance between the robot and the obstacle, and β is the weight of the obstacle distance score; velocity(v, ω) is the velocity score obtained by measuring the difference between the velocity of the robot and the optimal velocity, and γ is the weight of the velocity score; the sub-evaluation function astar(v, ω) represents the predicted trajectory distance score obtained by measuring the difference between the robot's position and the global path information, and δ is the weight of the sub-evaluation function astar(v, ω).

[0116] The expression of the sub-evaluation function astar(v, ω) is:

[0117] astar(v, ω) = h1 + h2

[0118] where h1 represents the Euclidean distance between the current position of the robot and the end point of the predicted trajectory, and h2 represents the Euclidean distance between the end point of the predicted trajectory and the target point.

[0119] To verify the effectiveness of the proposed GRRT*-DWA algorithm of the present invention, three 100×100 simulation environments are designed using the MATLAB2022b programming platform to compare the path planning capabilities of the GRRT*-DWA algorithm with the RRT algorithm, the RRT* algorithm, and the RRT*-DWA algorithm. As Figure 6 shown, the simulation environments are divided into a simple environment, a complex environment, and a complex dynamic environment. The basic simulation parameters are set as shown in Table 1. It should be noted that the DWA in the RRT*-DWA algorithm refers to the DWA algorithm without adding the sub-evaluation function, rather than the improved DWA algorithm in the present invention.

[0120] Table 1 Path planning simulation parameters

[0121]

[0122] The RRT, RRT*, and RRT*-DWA algorithms each have their own advantages: the RRT algorithm has the ability to quickly explore, the RRT* algorithm has strong path optimization ability, and the RRT*-DWA algorithm has high adaptability and real-time performance.

[0123] By selecting the RRT, RRT*, and RRT*-DWA algorithms as benchmarks and conducting experimental comparisons of the four algorithms, namely RRT, RRT*, RRT*-DWA, and GRRT*-DWA, in the same environment, the performance and applicability of the GRRT*-DWA algorithm proposed in the present invention can be comprehensively evaluated in various scenarios. By using three different maps, it is ensured that the GRRT*-DWA algorithm proposed in the present invention is comparable to other comparison algorithms in complex and severe environments. To avoid the contingency of the experiment, the following result comparisons are the averages of 100 successful experiments for each algorithm.

[0124] First, the performance of the four algorithms, namely RRT, RRT*, RRT*-DWA, and GRRT*-DWA, is tested in a simple environment, and the results are as Figure 7 shown. Although the RRT algorithm is efficient, the planned path is the worst and much longer than the paths planned by the other three algorithms; although the RRT* and RRT*-DWA algorithms have made optimization processes, due to the low efficiency of the optimization process, the final paths still have the problems of twists and turns and being long, and in the context of limiting the maximum number of iterations, there is a high failure rate; the performance of the GRRT*-DWA algorithm proposed in the present invention is significantly better than the other three algorithms, and the success rate has been greatly improved. This advantage is attributed to the adoption of multiple strategies to improve the efficiency of GRRT* path optimization. As shown in Table 2, it shows the Figure 7 performance comparison of the four algorithms, namely RRT, RRT*, RRT*-DWA, and GRRT*-DWA, in a complex environment.

[0125] Table 2 Performance comparison of the four algorithms in a simple environment

[0126]

[0127]

[0128] Next, the performance of the four algorithms, namely RRT, RRT*, RRT*-DWA, and GRRT*-DWA, is tested in a complex environment, and the results are as Figure 8As shown in the figure. The results of the RRT algorithm in a complex environment are still the worst, with obvious twists and turns; the RRT* and RRT*-DWA algorithms are superior to the RRT algorithm, but still do not select the optimal path near obstacles; while the GRRT*-DWA algorithm, due to adopting a feedback-biased sampling strategy, although the global optimal path is close to obstacles, obstacle avoidance processing is achieved. Despite the increased environmental complexity, the GRRT*-DWA algorithm still continues to generate paths that are closer to the asymptotically optimal route, demonstrating its robustness and efficiency in more challenging scenarios. As shown in Table 3, it shows Figure 8 the performance comparison of four algorithms, namely RRT, RRT*, RRT*-DWA, and GRRT*-DWA, in a complex environment.

[0129] Table 3 Performance comparison of four algorithms in a complex environment

[0130]

[0131] Finally, considering that after global path planning for a mobile robot, local obstacle avoidance still needs to be achieved, a complex dynamic environment is selected for performance testing, and the performance of three algorithms, namely DWA, RRT*-DWA, and GRRT*-DWA, is tested in a complex dynamic environment. The simulation results are as Figure 9 shown, where the black dashed line represents the movement trajectory of the dynamic obstacle. Among them, although the DWA algorithm is efficient, by mainly using the orientation evaluation sub-function of the DWA algorithm to guide the robot to the target point, the obstacles encountered along the way are likely to cause the mobile robot to deviate far from the target point, resulting in a longer final path; while the RRT*-DWA and GRRT*-DWA algorithms guided by the global path can both travel along the global optimal path and avoid dynamic obstacles at the same time. However, the GRRT*-DWA algorithm proposed in the present invention benefits from the GRRT* algorithm, so the generated global path is better, thus making the overall path of the mobile robot closer. And the improved DWA algorithm prevents the path from falling into a local optimum, so it is superior to the path planned by the RRT*-DWA algorithm in terms of overall effect.

[0132] As shown in Table 4, it shows Figure 9 the performance comparison of three algorithms, namely DWA, RRT*-DWA, and GRRT*-DWA, in a complex dynamic environment. Under the guidance of the global path, the efficiency of the GRRT*-DWA algorithm is increased by 56.5% compared with the RRT*-DWA algorithm, and the average length of the final path is increased by 4.6% compared with the DWA algorithm. The success rate of GRRT*-DWA is the best among the three algorithms and is much higher than the other two algorithms.

[0133] Table 4 Performance comparison of three algorithms in a complex dynamic environment

[0134]

[0135]

[0136] In summary, the GRRT*-DWA algorithm proposed in this paper is superior to the existing algorithms in terms of efficiency and path planning in both static and dynamic environments, and has high robustness and effectiveness.

[0137] Therefore, the following conclusions can be drawn:

[0138] The GRRT* algorithm obtained by improving the sampling strategy of RRT*, as well as node selection and step size adaptation, can detect the target area more quickly. And through the path point screening of the greedy algorithm, a large number of redundant paths are deleted, reducing the detection time and cost. The improved DWA algorithm can help the mobile robot achieve real-time obstacle avoidance during the path search process, ensuring the safety and effectiveness of the path. The GRRT*-DWA algorithm combines the advantages of the GRRT* algorithm and the improved DWA algorithm. Through simulation in three different environments using the MATLAB program, it is shown that the GRRT*-DWA algorithm is superior to the traditional methods and other improved algorithms in terms of path optimization degree, planning efficiency, and effectiveness, and has high robustness and effectiveness. The GRRT*-DWA algorithm can effectively handle complex environments containing static and dynamic obstacles, providing strong support for the path planning application of mobile robots.

[0139] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A path planning method for a mobile robot based on the GRRT*-DWA algorithm, characterized in that, It includes the following steps: Step 1, construct the GRRT*-DWA algorithm: generate the GRRT* algorithm; add a sub-evaluation function to the DWA algorithm to obtain an improved DWA algorithm; fuse the GRRT* algorithm with the improved DWA algorithm to obtain the GRRT*-DWA algorithm; Step 2, perform path planning for the mobile robot through the GRRT*-DWA algorithm: perform global path planning through the GRRT* algorithm; Based on the global path planning, perform optimal local planning through the improved DWA algorithm; complete the path planning.

2. The path planning method of the mobile robot based on the GRRT*-DWA algorithm according to claim 1, characterized in that, The method for generating the GRRT* algorithm is: first introduce a feedback bias sampling strategy, an expanded node filtering strategy, and an adaptive step size strategy into the RRT* algorithm, and then fuse the greedy algorithm.

3. The path planning method of the mobile robot based on the GRRT*-DWA algorithm according to claim 2, wherein The method for performing global path planning through the GRRT* algorithm is: sample nodes according to the feedback bias sampling strategy, filter nodes according to the expanded node filtering strategy, and search for an initial global path according to the adaptive step size strategy; Taking the path nodes of the initial global path as a reference, use the greedy algorithm to achieve optimal path planning and output the global optimal path planning; The method for performing optimal local planning through the improved DWA algorithm is: based on the global optimal path planning, extract the path nodes of the global optimal path planning as the path traction points of the improved DWA algorithm, constrain the optimized path not to deviate from the total global path, and select the optimal local planning trajectory based on the evaluation function with the added sub-evaluation function, and output the optimal local path.

4. The path planning method for a mobile robot based on the GRRT*-DWA algorithm according to claim 3, wherein The method for sampling nodes according to the feedback bias sampling strategy is: (1), Set the starting point as X init , Generate nearby nodes X of the starting point in the direction of the end point near , Nearby nodes X near Expand to generate child nodes X new , And generate a random point X rand ; (2) If the random point X rand does not trigger collision detection and is within the obstacle, it is the first new random point X rand_1 , and the child node X new obtains the angle information θ of the first new random point X rand_1 , and reselects the expansion area, discarding the left and right expansions in the angle information θ; (3) Repeat the process described in step (2) until the expansion of the random point X rand is no longer within the obstacle, which is the second new random point X rand_2 , and reselect a random point X new from the child node X rand .

5. The path planning method of the mobile robot based on the GRRT*-DWA algorithm according to claim 4, wherein The expression of the angle information θ is: Among them, the child node X new has coordinates (x new , y new ), and the random point X rand has coordinates (x rand , y rand ); When the starting point searches for a random point and encounters an obstacle, a random offset value will be generated. The expression of the offset value is: Β = Rand(-0.1, 0.01) where B is the offset value and Rand is the random number function.

6. The path planning method for a mobile robot based on the GRRT*-DWA algorithm according to claim 4, wherein The method for filtering nodes according to the expanded node filtering strategy is: Assume that the node to be expanded is X wait , first determine the node X to be expanded wait whether it can pass the collision detection. If it fails, discard the node X to be expanded that fails the collision detection wait ; If passed, calculate the cost function of the node X to be expanded through collision detection according to the Euclidean distance wait : f(X wait ) = d(X wait , X init ) + d(X wait , X goal ) Among them, f(X wait ) is the cost function of the node X to be expanded through collision detection wait , d is the function for calculating the Euclidean distance between two points, and X goal is the end point of the path; If the current node X to be expanded wait has a cost function f(X wait ) less than the path cost of the current path, then add the current node X to be expanded wait to the tree nodes; otherwise, directly filter and resample.

7. The path planning method for a mobile robot based on the GRRT*-DWA algorithm according to claim 6, characterized in that The method for searching for a path according to the adaptive step size strategy is: At the beginning of the GRRT* algorithm, set the initial step size according to the size of the map environment used, and set the minimum step size to half of the initial step size; At the beginning, use the initial step size to search for an initial path; As the number of iterations increases, gradually reduce the initial step size to the minimum step size using the global adaptive step size factor. After each initial step size is reduced, use the reduced initial step size to search for a path.

8. The path planning method for a mobile robot based on the GRRT*-DWA algorithm according to claim 7, wherein The method for taking the path nodes of the initial global path as a reference, using the greedy algorithm to achieve optimal path planning, and outputting the global optimal path planning is: ①Starting from the path end point X goal , assume that the previous path point closest to the path end point X goal is X n . Connect the path end point X goal with the path point X n . If the connection line between the path end point X goal and the path point X n does not collide with obstacles, then remove the path point X n . Connect the path end point X goal with the previous path point X n closest to the path point X n-1 . If the connection line between the path end point and path point X n-1 collides with an obstacle, then retain path point X n and make the retained path point X n become the new parent node; ② Repeat the process described in step ① until the path reaches the end point X goal or the new parent node is connected to the starting point X init and then stop.

9. The path planning method of the mobile robot based on the GRRT*-DWA algorithm according to claim 7, characterized in that, The expression of the global adaptive step size factor is: step = k × step0 where a and b are both parameters for controlling step size adjustment, t and T are the current iteration number and the maximum iteration number respectively, step and step0 are the current step size and the initial step size respectively, and exp is the exponential function.

10. The path planning method of a mobile robot based on the GRRT * - DWA algorithm according to any one of claims 2 - 9, characterized in that, The expression of the evaluation function with the added sub-evaluation function is: G(v, ω) = α * normal(heading(v, ω)) + β * normal(obs_dist(v, ω)) + γ * normal(velocity(v, ω)) + δ * normal(astar(v, ω)) Where normal() represents the normalization function; v is the linear velocity of the robot, ω is the angular velocity of the robot; heading(v, ω) is the target direction score obtained by measuring the angle between the current moment and the next arrival moment of the robot and the end point, and α is the weight of the target direction score; obs_dist(v, ω) is the obstacle distance score obtained by measuring the distance between the robot and the obstacle, and β is the weight of the obstacle distance score; velocity(v, ω) is the velocity score obtained by measuring the difference between the velocity of the robot and the optimal velocity, and γ is the weight of the velocity score; δ is the weight of the sub-evaluation function astar(v, ω); The expression of the sub-evaluation function astar(v, ω) is as follows: astar(v, ω) = h1 + h2 Where h1 represents the Euclidean distance between the current position of the robot and the end point of the predicted trajectory, and h2 represents the Euclidean distance between the end point of the predicted trajectory and the target point.

Images