Mobile robot path planning method based on improved DWA

Abstract

The invention provides a mobile robot path planning method based on an improved DWA, and the method comprises the steps: firstly determining global path planning through a Dijkstra algorithm, i.e., enabling a node which starts from a source node and is expanded each time to be a node of a currently known shortest path; a Dijkstra algorithm is improved, when a mobile robot encounters an obstacle, a global path regeneration mode is replaced by local optimization, a rotation angle of a path is smoothed, the speed of the robot is rapidly adjusted by using a DWA algorithm so as to avoid the sudden obstacle, and the DWA algorithm is improved so as to avoid the sudden obstacle. The speed weight in the DWA is dynamically adjusted according to the distance between the obstacle and the mobile robot, and the improved DWA algorithm and the improved Dijkstra algorithm are fused, so that the obstacle avoidance efficiency is high.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot path planning, and particularly relates to a mobile robot path planning method based on improved DWA. Background Art

[0002] Robot path planning has broad application prospects in highly automated scenarios such as intelligent manufacturing. Although the Dijkstra algorithm can search for the globally optimal path, its efficiency is low and the path has many turns, making it difficult to meet the real-time and smoothness requirements in complex environments. In practical applications, robots need to quickly respond to dynamically changing environments and simultaneously generate smooth and efficient motion paths to reduce energy consumption and mechanical wear. Aiming at the problems of frequent turning and low global planning efficiency of the Dijkstra algorithm in path planning, a method of fusing and improving the Dijkstra algorithm and the DWA algorithm is applied to mobile robots to solve the problems of dynamic obstacle avoidance or static obstacle avoidance in path planning.

[0003] For example, in the patent document with the patent application number 202311419045.8 and the publication date of January 26, 2024, a hybrid path planning method based on an improved A* algorithm and a dynamic window method is disclosed, belonging to the technical field of robot path planning. This method combines global prediction and local real-time planning into a hybrid two-layer algorithm, that is, an improved A* algorithm is used to complete global prediction to obtain globally optimal path nodes; then inflection points are selected from these nodes as the guidance for real-time planning. The real-time planning layer uses an improved dynamic window method to dynamically avoid obstacles in real time, continuously runs each local target point, and finally reaches the target point to complete the task. By using the improved A* algorithm, the shortest path can be found in the search space; at the same time, by using the optimized dynamic window method for local path planning, the window size can be adjusted according to the changes in the dynamic environment, thus ensuring the safety of the path and improving the practicability and flexibility of the dynamic obstacle avoidance method.

[0004] In the above literature, although it mentions coping with dynamic obstacles by improving the A* algorithm such as DWA, it conducts global prediction of the path through the improved A* algorithm, resulting in the need to predict the entire path each time, thus having a long path planning time. In addition, when adjusting local obstacles through the dynamic window method, it considers adjusting the node observation probability based on the distance value between nodes, resulting in a high computational complexity. Moreover, the method of adjusting the distance value cannot solve the speed control problem during the adjustment process. For example, in general adjustments, the speed when reaching the obstacle position is set to 0, which requires more driving distance to make the speed reach 0, thus resulting in a large driving distance required for obstacle avoidance and low obstacle avoidance efficiency. Summary of the Invention

[0005] The present invention provides a mobile robot path planning method based on improved DWA, which can improve the efficiency of path planning, shorten the running distance when avoiding obstacles, and has a high obstacle avoidance efficiency.

[0006] To achieve the above object, the technical solution of the present invention is: a mobile robot path planning method based on improved DWA for mobile robot path planning, and the specific steps include: S1 Preset the starting point and the ending point to form an initial path; S2 For the detected collision risk path segment, re-determine the collision risk path segment through the improved Dijkstra algorithm to form a new global path; S3 Smooth the turning angles of the new global path; S4 Use the evaluation function in the improved DWA algorithm to adjust the speed of the robot according to the distance between the obstacle and the mobile robot; S41 The evaluation function of the improved DWA algorithm is wherein according to the distance between the mobile robot and the obstacle dynamically adjust the weight value of , , ; The function used to evaluate the deviation between the target direction and the simulated trajectory end at the current speed; Is the distance function between the mobile robot and the obstacle object on the motion trajectory, Is the evaluation function of the magnitude of the current motion speed of the mobile robot, Is the current speed The weight coefficient of Is the distance between the robot and the obstacle object The weight coefficient of Is the normalization parameter of the three sub-functions; x And y Are the coordinates of the current point, And Are the coordinates where the obstacle is located; With the above settings, the improved Dijkstra algorithm is used to re-plan the local path with obstacles in the global path and add it to the global path, thereby improving the efficiency and real-time performance of path planning. In addition, by considering the distance of the obstacle and the speed of the robot, the weight value of is dynamically adjusted . The first case: when the distance between the robot and the obstacle is less than the radius of the obstacle, The value of is 0, so The value of is relatively large, and at this time, it is necessary to immediately adjust the speed of the mobile robot to avoid hitting the obstacle; The second case: When the distance between the robot and the obstacle is greater than the radius of the obstacle and less than four times the radius of the obstacle, The value of is x and y are the coordinates of the current point, is the coordinate where the obstacle is located. Therefore, the current value becomes smaller compared to the first case. At this time, as the distance decreases, the mobile robot needs to gradually increase the speed variable, which makes the difference between the current speed and the target speed increase. The current speed will not directly become the target speed of 0, so a greater running distance is required to directly reduce the speed to 0 within a short time, so as to achieve gradual adjustment to avoid obstacles; The third case: When the distance between the robot and the obstacle is greater than four times the radius of the obstacle, The value of is 3. The current value becomes smaller compared to the second case, indicating that the mobile robot maintains a safe distance from the obstacle and can continue the current behavior; The above dynamic adjustment can help the robot or object decelerate or change direction when approaching the obstacle, so as to avoid collisions, and resume the normal speed when moving away from the obstacle;

[0007] Furthermore, the formation of the initial path in step S2 includes: First, divide all nodes into two sets M and N. One set M contains one node, and the shortest path of the set M that has been determined initially has only the source node; The other set N contains the remaining nodes; Path update: In each iteration, select a node from the set N. The distance from this node to the source node is the shortest known so far. Add this node to the set M and update the shortest path estimate value of its adjacent nodes to the source node; Iteration process: Repeat the above process until the set N is empty, that is, the shortest paths of all nodes have been determined.

[0008] With the above settings, by dividing the nodes into two sets, the set M of the shortest paths that have been determined and the set N of those that have not, the algorithm can systematically and gradually construct the shortest paths until the shortest paths of all nodes are determined. In this way, the local paths with collision-risk path segments are determined to form the global path.

[0009] Furthermore, in step S2, the process of determining the path by improving the Dijkstra algorithm includes: first generating an initial path according to a preset static global cost map, then optimizing the starting path according to the dynamic local cost map at a fixed frequency, checking whether the current path segment intersects with the obstacles in the local cost map. For the detected collision-risk path segments, use the Dijkstra algorithm or other efficient path search algorithms to recalculate a safe path segment that avoids obstacles in the local cost map, and replace the original collision-risk path segment with the recalculated safe path segment.

[0010] With the above settings, the improved Dijkstra algorithm provides a path planning method that is both safe and efficient by combining global and local information and the ability to dynamically adapt to environmental changes. It is applicable to various dynamic environments and real-time applications, ensuring the safety and continuity of the entire path.

[0011] Furthermore, in step S3, smoothing the turning angles of the path includes: using the cubic Bezier curve method where t is a parameter, the starting point is and the ending point is , and the two control points .

[0012] With the above settings, using the cubic Bezier curve method to smooth the path can improve the smoothness, continuity, accuracy and flexibility of the path, thus enhancing the driving performance of the robot.

[0013] Furthermore, in step S41, the obstacle radius R is set to 0.5m, and according to the obstacle distance, A = 0.4 and B = 0.1 are set, then The range is 0.1 to 0.4.

[0014] With the above settings, more effective obstacle avoidance processing can be carried out within the preset range.

[0015] Furthermore, step S2 also includes: if there are more than two close obstacles in the local cost map, regard the more than two obstacles as a whole for obstacle avoidance.

[0016] With the above settings, when there are multiple close obstacles, the multiple obstacles can be regarded as a whole, in order to reduce the time for the robot to bypass the obstacles and improve the efficiency of path planning. Further, before step S4, it further includes: detecting whether the current node is the end point. If not, step S4 is performed; if so, the path planning is ended.

[0017] With the above settings, it is necessary to detect whether the current node is the end point before performing obstacle avoidance processing. If it is the end point, no obstacle avoidance processing is performed.

[0018] Further, in S5, if the global path contains the end point, the path planning is ended.

[0019] With the above settings, the path planning can be continuously performed. Description of the Drawings

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

[0021] Figure 2 It is a comparison diagram of path planning between the traditional Dijkstra algorithm and the improved Dijkstra algorithm in the present invention.

[0022] Figure 3 It is an effect diagram of path smoothing optimization in the present invention.

[0023] Figure 4 It is the yaw angle error of the robot without fusion in the present invention.

[0024] Figure 5 It is a planning diagram of the path of the traditional DWA algorithm with a size of 30*30m in the present invention.

[0025] Figure 6 It is a planning diagram of the path of the improved DWA algorithm with a size of 30*30m in the present invention.

[0026] Figure 7 It is a planning diagram of the path of the traditional DWA algorithm with a size of 40*40m in the present invention.

[0027] Figure 8 It is a planning diagram of the path of the improved DWA algorithm with a size of 40*40m in the present invention.

[0028] Figure 9 It is a simulation diagram of the static obstacle avoidance path before fusion in the present invention.

[0029] Figure 10 It is a simulation diagram of the static obstacle avoidance path after fusion in the present invention.

[0030] Figure 11 It is a simulation diagram after adding obstacles after fusion in the present invention.

[0031] Figure 12 It is a simulation diagram of the obstacle avoidance driving route after fusion in the present invention.

[0032] Figure 13This is the graph of the change in the moving speed of the robot in the x and y directions in the present invention.

[0033] Figure 14 This is the yaw angle error graph after the robot is fused in the present invention.

[0034] Figure 15 This is the graph of the rotational speed transformation after the robot is fused in the present invention.

[0035] Figure 16 This is the positioning accuracy test graph of the robot returning to the origin after being fused in the present invention.

[0036] Figure 17 This is the statistical graph of the repeat positioning accuracy after the robot is fused in the present invention. Detailed implementation manners

[0037] Example 1.

[0038] As Figure 1-17 shown, the path planning of the mobile robot by fusing the improved Dijkstra algorithm and the DWA dynamic window method is used for the path planning of the mobile robot. The specific steps include: S1 Preset the starting point and the ending point to form an initial path; S2 For the detected collision risk path segment, re-determine the collision risk path segment through the improved Dijkstra algorithm to form a new global path; S3 Smooth the turning angles of the new global path, such as using the Bezier curve method and the interpolation method; In this embodiment, the cubic Bezier curve method where t is a parameter, the starting point is , the ending point is , and the two control points .

[0039] S4 Use the evaluation function in the improved DWA algorithm to adjust the speed of the robot according to the distance between the obstacle and the mobile robot to avoid sudden obstacles. S41 The evaluation function of the improved DWA algorithm is where dynamically adjust according to the distance between the mobile robot and the obstacle the weight value of , , , The function for evaluating the deviation between the target direction and the simulated trajectory end at the current speed; is the distance function between the mobile robot and the obstacle object on the motion trajectory, is the evaluation function of the current motion speed magnitude of the mobile robot, is the current speed The weight coefficient, is the distance between the robot and the obstacle object The weight coefficient, is the normalization parameter of the three sub-functions; x and y are the coordinates of the current point, are the coordinates where the obstacle is located.

[0040] In this embodiment, it is set that the obstacle radius R = 0.5m. According to the obstacle distance, A = 0.4 and B = 0.1 are set, then The range is 0.1~0.4.

[0041] S5 If the global path contains the end point, the path planning ends. If it does not contain the end point, continue to drive.

[0042] By considering the distance to the obstacle and the speed of the robot, the weight value is dynamically adjusted , the first case: when the distance between the robot and the obstacle is less than the obstacle radius, The value of is 0. Therefore The value of is larger. At this time, it is necessary to immediately adjust the speed of the mobile robot to avoid hitting the obstacle; The second case: when the distance between the robot and the obstacle is greater than the obstacle radius and less than four times the obstacle radius, The value of is , where x and y are the coordinates of the current point, and are the coordinates where the obstacle is located. Therefore, the current The value of becomes smaller compared to the first case. At this time, the mobile robot needs to gradually increase the speed variable as the distance decreases, so that the difference between the current speed and the target speed increases. The current speed will not directly become the target speed 0, so a greater running distance is required to directly reduce the speed to 0 within a short time, so as to achieve gradual adjustment to avoid obstacles; The third case: when the distance between the robot and the obstacle is greater than four times the obstacle radius, The value of is 3. The current The value becomes smaller compared to the second case, indicating that the mobile robot maintains a safe distance from the obstacle and can continue the current behavior; The above dynamic adjustment can help the robot or object decelerate or change direction when approaching the obstacle, so as to avoid collision, and resume the normal speed when moving away from the obstacle; Therefore, at the global path planning level, by optimizing the evaluation mechanism of the Dijkstra algorithm, the search efficiency is enhanced, unnecessary node processing is reduced, and thus the planning time is shortened. At the local path planning level, an improved DWA algorithm is adopted and the algorithm process is integrated to optimize the path planning.

[0043] The formation of the initial path in step S2 includes: first, all nodes are divided into two sets M and N. One set M contains a single node, and the shortest path starting from only the source node has been determined for set M. The other set N contains the remaining several nodes. Path update: In each iteration, a node is selected from set N whose distance to the source node is currently the shortest known. This node is added to set M and the shortest path estimate from its adjacent nodes to the source node is updated. Iteration process: Before set N is empty, that is, before the shortest paths of all nodes are determined, the above process is repeated. By dividing the nodes into two sets, the set M with determined shortest paths and the set N with undetermined shortest paths, the algorithm can systematically and gradually construct the shortest path until the shortest paths of all nodes are determined.

[0044] As Figure 2 shown, in step S2, the process of the improved Dijkstra algorithm for determining the path includes: first, generating an initial path according to a preset static global cost map, and then optimizing the starting path according to the dynamic local cost map at a fixed frequency, checking whether the current path segment intersects with the obstacles in the local cost map. For the detected path segments with collision risks, use the Dijkstra algorithm or other efficient path search algorithms to recalculate a safe path segment that avoids obstacles in the local cost map, and replace the original path segment with collision risks with the recalculated safe path segment to ensure the safety and continuity of the entire path.

[0045] When there are multiple nearby obstacles in the local cost map, that is, the distance between multiple obstacles is less than a preset value, the obstacles can be regarded as a whole. That is, the center of multiple obstacles is taken as the coordinate point of multiple obstacles, and the area containing multiple obstacles forms an obstacle. In order to reduce the time for the robot to bypass obstacles and improve the efficiency of path planning, the improved Dijkstra algorithm provides a path planning method that is both safe and efficient by combining global and local information and the ability to dynamically adapt to environmental changes, and is applicable to various dynamic environments and real-time applications.

[0046] As Figure 3 shown, in step S3, the corners of the path are smoothed using the cubic Bezier curve method where t is a parameter, the starting point is , the end point is , and the two control points By using the cubic Bezier curve method to smooth the path, the smoothness, continuity, accuracy, and flexibility of the path can be improved, thus enhancing the driving performance of the robot.

[0047] To avoid data contingency, multiple random experiments were conducted. Another fifteen groups of coordinates were randomly selected, and the specific content is shown in Table 2. Meanwhile, for the path length and the number of yaw angle oscillations before and after optimization, the traditional algorithm and the optimized algorithm were respectively used for path planning, and the relevant data are shown in Table 3.

[0048] As Figure 5 、 6 and Table 1 show, for local path planning, Figure 5 Figure (a) is the path planning diagram of the traditional DWA algorithm, Figure 6 Figure (b) is the path planning diagram of the improved DWA algorithm. It can be seen that the improved DWA algorithm runs 25.06% shorter in distance, reduces the number of path turns by 40%, and shortens the running time by 21.98% compared with the traditional DWA algorithm in a 30*30 map.

[0049]

[0050] As Figure 7 、 8 and Table 2 show, Figure 7 Figure (c) is the running diagram of the traditional DWA algorithm in a 40*40 graph, Figure 8 Figure (d) is the running diagram of the improved DWA algorithm in a 40*40 graph. In a 40*40 graph, the running distance is reduced by 6.25%, the number of path turns is reduced by 20%, and the running time is reduced by 14.43%.

[0051]

[0052] As Figure 9 、 10 and Table 3 show, Figure 9 Figure (e) is the schematic diagram of the running distance of the running path of the static obstacle by fusing the improved DWA algorithm and the improved Dijkstra algorithm, Figure 10 Figure (f) is the schematic diagram of the running distance of the running path of the static obstacle by fusing the DWA algorithm and the Dijkstra algorithm. The fused algorithm shortens the running distance of the static obstacle by 15.08%, reduces the number of path turns by 33%, and reduces the running time by 15.30%.

[0053]

[0054] As Figure 11 、 12 and Table 4 show, Figure 11Schematic diagram of the running distance of the running path of a dynamic obstacle by integrating and improving the DWA algorithm and the improved Dijkstra algorithm Figure 12 Schematic diagram of the running distance of the running path of a dynamic obstacle by integrating the DWA algorithm and the Dijkstra algorithm. After integration, the algorithm reduces the running distance of dynamic obstacles by 15.11%, the number of path turning times by 50%, and the running time by 13.16%.

[0055]

[0056] As Figure 13-15 shown, after using this method, the graphs of the moving speed of the mobile robot in the XY direction, the error of the yaw angle, and the change of the rotation speed. When the mobile robot encounters an obstacle, the linear speed rises gently to 200 mm / s in the x direction and then decreases gently and is continuously adjusted. The linear speed of the mobile robot will decrease and the speed of the mobile robot will be continuously adjusted. If there is an obstacle for the mobile robot, the change of the angular speed is obvious, and the error of the navigation yaw angle is very small, which proves the real-time adjustment of the obstacle avoidance line by the fusion algorithm, thus ensuring the path safety of the mobile robot. The change of the azimuth angle of the mobile robot is relatively gentle and there is no abrupt change, which indicates that the route change planned by the fusion algorithm that meets the application requirements of the mobile robot is stable.

[0057] As Figure 16-17 shown, the robot will laser mark its positioning accuracy at the origin every two round trips in this map. The first round trip is a static map experiment, and the second is a dynamic obstacle avoidance experiment. The robot has achieved relatively ideal positioning effects in multiple experiments. The maximum error in the x direction is 24 mm, and the maximum error in the y direction is 22 mm. It can be seen that the average error of the mobile robot in the X direction ≤ 24 mm, and the average error in the Y direction ≤ 22 mm. It proves that the movement of the mobile robot has higher positioning accuracy under the planning of the fusion algorithm.

[0058] The working principle of the present invention: First, the Dijkstra algorithm is used to determine the global path planning, that is, each expanded node starting from the source node is the node with the currently known shortest path; the Dijkstra algorithm is improved. When the mobile robot encounters an obstacle, the way of regenerating the global path is replaced by local optimization, the turning angle of the path is smoothed, and the DWA algorithm is used to quickly adjust the speed and direction of the robot to avoid sudden obstacles. The DWA algorithm is improved by dynamically adjusting the speed weight in the DWA according to the distance between the obstacle and the mobile robot, and the improved DWA algorithm is integrated with the improved Dijkstra algorithm to improve the obstacle avoidance efficiency.

Claims

1. A mobile robot path planning method based on improved DWA, used for mobile robot path planning, characterized in that: The specific steps include: S1 presets the starting point and the end point to form the initial path; S2 re-determines the detected collision risk path segment by improving the Dijkstra algorithm to form a new global path; S3 smoothes the corners of the new global path; S4 uses the evaluation function in the improved DWA algorithm to adjust the robot's speed according to the distance between the obstacle and the mobile robot; The evaluation function of the S41 improved DWA algorithm is: , where the distance between the mobile robot and the obstacle Dynamic Adjustment The weight value , A function for evaluating the deviation of the target direction from the simulated trajectory end at the current speed; is the distance function between the mobile robot and the obstacle object on the motion trajectory, is the evaluation function of the current movement speed of the mobile robot, Current speed The weight coefficient of is the distance between the robot and the obstacle The weight coefficient of are the normalized parameters of the three sub-functions; x and y are the coordinates of the current point, are the coordinates of the obstacle.

2. The mobile robot path planning method based on improved DWA according to claim 1, characterized in that: The initial path formation in step S2 includes: firstly dividing all nodes into two sets M and N, one of which is a set M containing a node, and the set M has determined the shortest path of only the source node initially; and a set N contains the remaining nodes; Path update: In each iteration, select a node from the set N whose distance to the source node is the shortest known so far, add this node to the set M and update the shortest path estimate from its adjacent nodes to the source node; Iteration process: Repeat the above process until the set N is empty, that is, until the shortest paths of all nodes have been determined.

3. The mobile robot path planning method based on improved DWA according to claim 1, characterized in that: In step S2, the improved Dijkstra algorithm determines the path process including: firstly, generating an initial path according to a preset static global cost map, then optimizing the starting path according to a dynamic local cost map at a fixed frequency, checking whether the current path segment intersects with obstacles in the local cost map, and for the detected collision risk path segment, using the Dijkstra algorithm or other efficient path search algorithms, recalculating a safe path segment that avoids obstacles in the local cost map, and replacing the original collision risk path segment with the recalculated safe path segment.

4. The mobile robot path planning method based on improved DWA according to claim 1, characterized in that: In step S3, the path corners are smoothed by using a cubic Bezier curve method. Where t is a parameter, the starting point is , the end point is , two control points .

5. The mobile robot path planning method based on improved DWA according to claim 1, characterized in that: Set the obstacle radius R = 0.5m, and set A = 0.4, B = 0.1 according to the obstacle distance. The range is 0.1~0.

4.

6. The mobile robot path planning method based on improved DWA according to claim 3 is characterized in that: Step S2 also includes: if there are two or more obstacles in close proximity in the local cost map, the two or more obstacles are regarded as a whole for obstacle avoidance.

7. The mobile robot path planning method based on improved DWA according to claim 1, characterized in that: Before step S4, the process also includes: detecting whether the current node is the end point, if not, proceeding to step S4, and if so, ending the path planning.

8. The mobile robot path planning method based on improved DWA according to claim 1, characterized in that: S5: If the global path contains the end point, the path planning ends.

Images