A path planning method for indoor mobile robots combining improved A* and DWA algorithms

Abstract

The present invention discloses a method for indoor mobile robot path planning that integrates improved A* and DWA algorithms, including: S1, optimizing the cost function of the traditional A* algorithm, introducing the environmental information obstacle rate Q into the cost function, changing the weight of the heuristic function H(n), and realizing its adaptive adjustment; S2, optimizing the path smoothness, using a key point selection strategy, solving the problem of redundant collinear nodes and turning points, retaining necessary path nodes, and obtaining a global path with only key points; S3, constructing an evaluation function combining key point information, and then applying the DWA algorithm to make the local path planning follow the global path profile, so that the path is smoother and real-time obstacle avoidance is realized. The present invention can quickly find the optimal path, and on the basis of global optimization, can timely avoid dynamic and static obstacles appearing in the environment, and improve the adaptability of indoor mobile robots in complex environments.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot navigation and path planning, and in particular to an indoor mobile robot path planning method integrating improved A* and DWA algorithms. Background Art

[0002] At present, path planning is one of the cutting-edge topics in the field of mobile robot research. Its purpose is to avoid obstacles in a complex environment and find the best path from the starting point to the end point. Path planning technology is also the core technology for mobile robots to achieve autonomous navigation. In order to solve the path planning problem, a large number of scholars at home and abroad have studied this topic and proposed various algorithms.

[0003] Path planning can be divided into two categories: global path planning and local path planning. Among them, the global path algorithms include A* algorithm, Dijkstra algorithm and RRT algorithm; the local path algorithms include DWA algorithm, particle swarm algorithm, artificial potential field method and ant colony algorithm.

[0004] The Dijkstra algorithm uses a traversal search method, with a large number of planning nodes, a very large node network and low algorithm efficiency. Based on the Dijkstra algorithm, the A* algorithm introduces the estimated cost from the target point to the current point, determines the path search direction according to the estimated cost, and improves the efficiency of the algorithm. The A* algorithm can quickly realize the collision-free and shortest global path planning of mobile robots in a known environment, mainly planning an optimal safe road through node state detection and simple valuation functions. However, the path planned by the A* algorithm is connected by nodes, resulting in discontinuous curvature and the disadvantage of a long path. Zhao Xiao et al. proposed an improved A* algorithm based on a jumping point search algorithm, which improved the path search speed and reduced the amount of calculation. Zhang et al. proposed an improved A* algorithm that traverses all nodes of the path and then deletes unnecessary nodes, shortening the robot's driving path and turning time. However, these improved A* algorithms only consider the optimization of the global path, but the robot cannot avoid unknown obstacles.

[0005] As for the local path planning algorithm, the DWA algorithm has good local obstacle avoidance capabilities. Wang Yongxiong et al. proposed a parameter-adaptive DWA algorithm, which obtained the optimal speed of robot movement and improved safety. Mai et al. proposed an improved DWA algorithm that can perceive the distribution of dense objects in advance, so that the robot can stably avoid dense areas. However, while the improved DWA algorithm can achieve robot obstacle avoidance, it cannot achieve global path optimization.

[0006] The above improved algorithms only consider the global path optimization or local obstacle avoidance during the navigation process of the mobile robot. Therefore, how to enable the robot to find the optimal path and avoid obstacles in real time during the navigation process has always been a problem that people in this technical field need to solve. Summary of the invention

[0007] The purpose of the present invention is to solve the above problems, and to realize that the mobile robot can not only travel on the global optimal path but also avoid obstacles in real time, and propose a path planning method for indoor mobile robots that integrates the improved A* and DWA algorithms. The algorithm introduces environmental information into the heuristic function of the traditional A* algorithm to improve the search efficiency; deletes redundant nodes in the trajectory and reduces turns to optimize the path smoothness; extracts the key points of the path planned by the improved A* algorithm as the intermediate target points of the DWA algorithm for global guidance, and realizes the integration of the improved A* and DWA algorithms.

[0008] The technical solution adopted by the present invention is: a method for indoor mobile robot path planning integrating improved A* and DWA algorithms, specifically comprising the following steps:

[0009] S1: Optimize the cost function of the traditional A* algorithm, introduce the environmental information obstacle rate Q into the cost function, change the weight of the heuristic function H(n), and realize its adaptive adjustment;

[0010] S2: Optimization of path smoothing, using a key point selection strategy to solve the problem of redundant collinear nodes and turning points, retaining necessary path nodes, and obtaining a global path with only key points;

[0011] S3: Construct an evaluation function that combines key point information, and then apply the DWA algorithm to make the local path planning follow the global path contour, thereby making the path smoother and achieving real-time obstacle avoidance.

[0012] Preferably, the step S1 optimizes the cost function of the traditional A* algorithm, specifically including introducing the environmental obstacle rate Q that changes with the current position of the mobile robot into the cost function F(n). Assuming that the number of rectangular grids formed by the robot's starting point and the target point is M, and the number of grid obstacles within the search range from the current node to the target point is N, the environmental obstacle rate expression is: The environmental obstacle rate Q is introduced into the cost function F(n), and the weight of the heuristic function H(n) is adaptively changed. The cost function F(n) is now: F(n) = G(n) + e Q H(n), thereby optimizing the traditional A* algorithm.

[0013] Preferably, the step S2 is to optimize the path smoothness. The traditional A* algorithm path planning is composed of continuous grid center points connected, there are many redundant nodes, the path has many turns, and the path is not smooth. In view of these problems, a path smoothing optimization algorithm is designed based on the Floyd algorithm. Specifically, it includes the following steps: Step 1: traverse all nodes, delete the redundant nodes in the middle of each path, and retain the starting point and turning point. Step 2: traverse the starting point and turning point, connect each node from the starting point to the subsequent node as an alternative path, and determine the distance d between each path and the obstacle grid. i Relationship with safety distance D. If d i ≤D, then delete the path if d i >D, keep the path and delete the inflection points between the paths. Step 3: Extract the remaining nodes, output the optimized path, and the algorithm ends.

[0014] Preferably, the improved A* algorithm of step S3 is integrated with the DWA algorithm, and steps S1 and S2 realize the improvement of the A* algorithm, and obtain a navigation path that only includes the starting point, key points and target points, but cannot avoid unknown obstacles appearing in the environment. The DWA algorithm has good local obstacle avoidance capabilities, but it only has one final target point as a guide, and it is easy to fall into local optimality. Therefore, the present invention integrates the two algorithms, and uses the global path key points extracted from the improved A* algorithm planning as the intermediate target points of the DWA algorithm. The optimized evaluation function makes the local path planning follow the planned global path contour. By fusing the navigation algorithm, the global path optimization in the navigation process of the mobile robot can be achieved, while having real-time obstacle avoidance function.

[0015] Beneficial effects of the present invention:

[0016] 1. Optimize the cost function of the traditional A* algorithm, realize its adaptive adjustment, and improve the efficiency of the algorithm;

[0017] 2. Solve the problem of redundant collinear nodes and turning points, retain necessary path nodes, obtain a global path with only key points, and achieve smooth path optimization;

[0018] 3. Integrate with the DWA algorithm to construct an evaluation function that combines key point information, and then apply the DWA algorithm to make the local path planning follow the global path contour, thereby making the path smoother and achieving real-time obstacle avoidance. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 A* algorithm search diagram for mobile robot path planning;

[0020] Figure 2 A schematic diagram of the search steps when the current point selects the next point during path planning for a mobile robot;

[0021] Figure 3 This is a flow chart of a path planning method for an indoor mobile robot according to an embodiment of the present invention;

[0022] Figure 4a This is a schematic diagram of a path before smoothing optimization according to an embodiment of the present invention;

[0023] Figure 4b This is a schematic diagram of a path after smoothing optimization according to an embodiment of the present invention;

[0024] Figure 5 4 is a flow chart of a fusion algorithm according to an embodiment of the present invention. DETAILED DESCRIPTION

[0025] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments.

[0026] In order to fully understand the technical solution of the present invention, the A* algorithm is briefly introduced as follows:

[0027] The traditional A* algorithm search principle is to add the starting point to the open list, add the point as the parent node to the close list, search for its adjacent reachable nodes and add them to the open list. Calculate the cost of the nodes in the open list according to the cost function, select the node with the lowest cost as the next parent node and put it in the close list, search for the parent node reachable nodes again and calculate their costs, and repeat the cycle until the parent node is at the target point.

[0028] The cost function of the traditional A* algorithm is: F(n) = G(n) + H(n). In the formula, n represents the current node, and F(n) represents the cost function of the mobile robot at the current node, which is used to select the optimal path. G(n) represents the actual cost of the mobile robot from the starting point to the current point. H(n) is a heuristic function, which represents the estimated cost from the current point n to the target point. In general, H(n) is less than the actual cost from the current point n to the target point. When the H(n) value is 0, only G(n) works, and the algorithm is the Dijkstra algorithm. When the estimated value of H(n) is smaller than the actual cost, the algorithm search time will increase due to the increase in the search space. When the estimated value of H(n) is larger than the actual cost, the algorithm search speed will increase, but the algorithm may not be able to search for the shortest path.

[0029] It is not difficult to see that H(n) has a great impact on search efficiency. H(n) has several common forms: (1) Manhattan distance; (2) Chebyshev distance; (3) Euclidean distance. A specific embodiment of the present invention uses Manhattan distance.

[0030] like Figure 1 As shown in the figure, it is the search area, where the black grids are obstacle nodes and the white grids are walkable nodes. Figure 2 The search steps for selecting the next point for the current point, assuming that the straight line cost is 10 and the diagonal line cost is 14, such as Figure 2 As shown in the figure, the cost function of the lower right corner is the smallest, so the next node is the lower right corner node, and so on. If an obstacle is encountered, the obstacle will not be considered. It stops until it reaches the target point and obtains the shortest path, as shown in the figure below. Figure 1 As shown, the paths connected by broken lines represent the shortest paths found.

[0031] The invention discloses an indoor mobile robot path planning method integrating improved A* and DWA algorithms, the core of which is to improve the A* algorithm and integrate the A* and DWA algorithms. The following embodiments are used to illustrate the invention, but are not used to limit the scope of the invention.

[0032] like Figure 3 As shown, the flow chart of the indoor mobile robot path planning method described in this embodiment specifically includes: S1, optimizing the cost function of the traditional A* algorithm, introducing the environmental information obstacle rate Q into the cost function, changing the weight of the heuristic function H(n), and realizing its adaptive adjustment; S2, optimizing the path smoothness, using a key point selection strategy to solve the problem of redundant collinear nodes and turning points, retaining the necessary path nodes, and obtaining a global path with only key points; S3, constructing an evaluation function combining key point information, and then applying the DWA algorithm to make the local path planning follow the global path contour, so that the path is smoother and real-time obstacle avoidance is realized.

[0033] S1, optimizes the cost function of the traditional A* algorithm, introduces the environmental information obstacle rate Q into the cost function, changes the weight of the heuristic function H(n), and realizes its adaptive adjustment.

[0034] When the current node is far from the target point, the estimated cost value is much smaller than the actual value. At this time, the search space is large and there are many search nodes. The weight of H(n) should be appropriately increased to improve the search efficiency. When the current node gradually approaches the target point, the estimated cost value also increases and approaches the actual value. In order to prevent the estimated value from being too large and falling into the local optimum, the weight of H(n) should be appropriately reduced. Therefore, in order to adaptively adjust the weight of the heuristic function, the environmental obstacle rate Q that changes with the current position of the mobile robot is introduced.

[0035] By introducing the environmental obstacle rate Q, the cost function of the traditional A* algorithm is optimized. The expression of the environmental obstacle rate is: Where M is the number of grids in the rectangular area composed of the starting point and the target point, and N is the number of grid obstacles in the search range from the current point to the target point.

[0036] The cost function F(n) is optimized to F(n) = G(n) + e QH(n). As the robot moves from the starting point to the target point, the environmental obstacle rate Q gradually decreases, so that the weight of H(n) gradually decreases. When the current node is far away from the target point, the weight is large; when the current node is close to the target point, the weight is small. By realizing the adaptive adjustment of H(n), the search efficiency of the robot in different positions is improved.

[0037] S2, the optimization of path smoothing, uses a key point selection strategy to solve the problem of redundant collinear nodes and turning points, retains necessary path nodes, and obtains a global path with only key points.

[0038] The traditional A* algorithm path planning consists of connecting continuous grid center points, with many redundant nodes, many path turns, and uneven paths. To address these problems, a path smoothing optimization algorithm is designed based on the Floyd algorithm.

[0039] The principle of path smoothing optimization is as follows Figure 4a and 4b As shown in the figure, the path planning of the traditional A* algorithm is composed of lines connecting the center points of the grid. The path before optimization is (S, 1, 2, ..., 13, E), and there are many redundant nodes. On the basis of considering the safety distance D, the path is smoothly optimized so that the path selection is no longer limited to the center position of the grid. The smoothness of the optimized path is increased, and the path length and turning points are reduced. The steps of path smoothing optimization are as follows:

[0040] Step 1: Traverse all nodes, delete the redundant nodes in the middle of each path, and keep the starting point and turning point. After deleting the middle points, there are nine nodes left: S, 2, 6, 8, 9, 10, 11, 13, and E.

[0041] Step 2: Traverse the starting point and turning point, connect each node from the starting point to the next node as an alternative path, and determine the distance d between each path and the obstacle grid i Relationship with safety distance D. If d i ≤D, then delete the path if d i > D, keep the path and delete the inflection points between the paths. After deleting unnecessary inflection points, four nodes S, 6, 8, and E remain.

[0042] Step 3: Extract the remaining nodes, output the optimized path, and the algorithm ends.

[0043] S3, constructs an evaluation function combining key point information, and then applies the DWA algorithm to make the local path planning follow the global path contour, thereby making the path smoother and achieving real-time obstacle avoidance.

[0044] The improved A* algorithm obtains a navigation path that only contains the starting point, key points and target points, but cannot avoid unknown obstacles in the environment. The DWA algorithm has good local obstacle avoidance capabilities, but it only has one final target point as a guide and is prone to falling into local optimality. Therefore, the present invention combines the two algorithms, using the global path key points extracted from the improved A* algorithm as the intermediate target points of the DWA algorithm, and the optimized evaluation function makes the local path planning follow the planned global path contour. By fusing the navigation algorithm, the global path optimization in the mobile robot navigation process can be achieved, while also having real-time obstacle avoidance capabilities.

[0045] The specific process of the fusion algorithm is as follows: Figure 5 As shown in the figure. First, the improved A* algorithm is used to plan a global path, and the path is optimized using its optimized evaluation function and key point selection strategy, and the necessary key nodes of the path are obtained. Then, the extracted path key points are used as the intermediate target points of the DWA algorithm to guide the planning of the local path. The DWA algorithm samples the speed of the mobile robot and simulates the moving trajectory of each speed pair. The optimal simulated moving trajectory is selected using the evaluation function combined with the key point information, and the robot is controlled to move to the target point at the speed corresponding to the optimal trajectory, thereby realizing the fusion of the improved A* algorithm and the DWA algorithm.

[0046] The above description of the embodiments is to facilitate the understanding and application of the present invention by those skilled in the art. Those skilled in the art may still modify or replace the specific embodiments of the present invention with equivalents, and any modification or equivalent replacement that does not depart from the spirit and scope of the present invention shall be included in the scope of the claims of the present invention.

Claims

1. A method for indoor mobile robot path planning integrating improved A* and DWA algorithms, characterized in that: The specific steps include: S1: Optimize the cost function of the traditional A* algorithm, introduce the environmental information obstacle rate Q into the cost function, change the weight of the heuristic function H(n), and realize its adaptive adjustment; S2: Optimization of path smoothing, using a key point selection strategy to solve the problem of redundant collinear nodes and turning points, retaining necessary path nodes, and obtaining a global path with only key points; S3: Construct an evaluation function that combines key point information, and then apply the DWA algorithm to make the local path planning follow the global path contour, thereby making the path smoother and achieving real-time obstacle avoidance; The step S1 optimizes the cost function of the traditional A* algorithm, specifically including introducing the environmental obstacle rate Q that changes with the current position of the mobile robot into the cost function F(n); Assuming that the number of rectangular grids formed by the robot's starting point and target point is M, and the number of grid obstacles within the search range from the current node to the target point is N, the environmental obstacle rate expression is: The environmental obstacle rate Q is introduced into the cost function F(n), and the weight of the heuristic function H(n) is adaptively changed; the cost function F(n) at this time is: F(n) = G(n) + e Q H(n), thereby optimizing the traditional A* algorithm; The step S2 of optimizing the path smoothness includes the following steps: Step 1: Traverse all nodes, delete redundant nodes in the middle of each path, and keep the starting point and turning point; Step 2: Traverse the starting point and turning point, connect each node with the following nodes from the starting point as an alternative path, and determine the distance d between each path and the obstacle grid i Relationship with safety distance D; If d i ≤D, then delete the path if d i >D, keep the paths and delete the turning points between the paths; Step 3: Extract the remaining nodes, output the optimized path, and the algorithm ends; The improved A* algorithm in step S3 is integrated with the DWA algorithm, and the global path key points extracted by the improved A* algorithm are used as the intermediate target points of the DWA algorithm. The optimized evaluation function makes the local path planning follow the planned global path contour; by integrating the navigation algorithm, the global path optimization in the navigation process of the mobile robot is achieved, while having the real-time obstacle avoidance function.

Images