Forklift local path planning method based on new speed evaluation space of DWA

Abstract

The application provides a forklift global path planning method based on a new speed evaluation space of DWA, which judges whether an obstacle will interfere with the movement of a mobile robot in a time period, reduces the number of times of path re-planning of the mobile robot, provides work efficiency and good smoothness; meanwhile, when the obstacle blocks the movement of the mobile robot, a local path is generated to avoid the obstacle. Meanwhile, the local path closest to the global path is selected as the optimal path; thus, the trajectory of the optimal path is consistent with the trajectory of the whole path, the smoothness of the optimal path is good, and interference of the mobile robot on another global path is avoided when the mobile robot moves along the optimal path; thus, the mobile robot does not interfere with another mobile robot moving between different initial nodes and different target nodes.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path planning, in particular to a forklift local path planning method based on a new type of speed evaluation space of DWA. BACKGROUND

[0002] As a typical mobile robot, the forklift is widely used in transportation and production, and has the advantages of long-time and high-efficiency work. However, the planning of the moving path of the forklift will determine the work efficiency of the forklift during the working process, so that the importance of the path planning method is highlighted.

[0003] As disclosed in the patent document with the Chinese application number 202211581793.1 and the publication date of 2023.2.24, a global moving method of a mobile robot is disclosed. In the process of approaching the target node from the initial node, the node with the minimum generation value is searched, and then the mobile robot moves before the node with the minimum generation value. The moving distance of the mobile robot is short, and the moving time is short. At the same time, for the known obstacles, the generation value of the nodes other than the obstacle nodes is calculated. The mobile robot moves to avoid the obstacle nodes, and the known obstacles are avoided. For the newly appeared obstacles, a local path is planned in the process of the mobile robot approaching the newly appeared obstacles, and the unknown obstacles are avoided.

[0004] When avoiding the newly appeared obstacles, the mobile robot deviates from the set trajectory to avoid the obstacles, and then the shortest path between the current node after avoiding the obstacles and the path end point is planned. When planning the path, the speed of the mobile robot is not considered, and at the same time, the walking path of the mobile robot is not smooth. At the same time, it is easy to interfere with the mobile robots on other paths. SUMMARY

[0005] The present application provides a forklift global path planning method based on a new type of speed evaluation space of DWA. Whether the obstacle will interfere with the movement of the mobile robot is judged within a time period, the number of times of re-planning the path of the mobile robot is reduced, the working efficiency is provided, and the smoothness is good. At the same time, when planning the local path, it is close to the global path, the smoothness is good, and at the same time, it avoids interference with the mobile robots on other paths.

[0006] To achieve the above purpose, the technical scheme of the present application is as follows: a forklift global path planning method based on a new type of speed evaluation space of DWA, comprising the following steps:

[0007] S1, presetting a map, constructing a global coordinate system in the map, and constructing a robot coordinate system with the midpoint of the mobile robot as the origin. Each coordinate point in the global coordinate system is a node.

[0008] The initial node and target node are preset, the initial node is the starting point of the mobile robot, and the target node is the end point of the mobile robot.

[0009] S2. Generate a global path between the initial node and the target node. The global path is the shortest path between the initial node and the target node.

[0010] S3, the mobile robot moves along the global path, approaching the target node from the initial node.

[0011] S4. Determine whether the mobile robot encounters an obstacle on the global path. If so, proceed to S5; if not, continue moving along the global path.

[0012] S5. Calculate the cost between each node on the global path and the obstacle, and set the node with a cost greater than zero to the obstacle as a temporary stop node; the mobile robot stops moving after moving to the temporary stop node.

[0013] S6. Preset a pause time, start timing the pause time, detect obstacles, and repeatedly calculate the cost value between the temporary stop node and the obstacle. If the cost value is equal to zero, it means that the obstacle has disappeared, and then proceed to S7; if the cost value between the temporary stop node and the obstacle is still greater than zero after the pause time ends, it means that the obstacle still exists, and then proceed to S8.

[0014] S7. The mobile robot continues to move along the global path.

[0015] S8. Generate one or more local paths that bypass obstacles.

[0016] S9. Evaluate the speed of more than one local path and select the local path closest to the global path as the optimal path.

[0017] S10: The mobile robot moves along the optimal path toward the target node.

[0018] The above method first generates a global path between the initial node and the target node, determining the shortest path between the initial node and the target node, allowing the mobile robot to quickly approach the target node. As the mobile robot approaches the target node, it scans for obstacles in real time. Upon identifying an obstacle, a temporary stopping point is calculated based on the cost value, and the mobile robot moves to the temporary stopping point to wait. The obstacle's position is detected in real time during the pause time. When the obstacle moves away from the mobile robot, the cost value between the mobile robot and the obstacle decreases, and no longer interferes with the mobile robot. The mobile robot then continues to move toward the target node without replanning its path, resulting in smoothness and computational simplicity. If the cost value between the mobile robot and the obstacle remains unchanged after the pause time, it indicates that the obstacle will interfere with the mobile robot's movement, allowing it to avoid the obstacle. By setting a pause time, whether an obstacle will interfere with the mobile robot's movement is determined within a certain period of time, reducing the number of times the mobile robot needs to replan its path. This approach is computationally simple and smooth.

[0019] When an obstacle blocks the mobile robot's progress, a local path is generated to avoid it. The local path that is closest to the global path is selected as the optimal path. This ensures that the trajectory of the optimal path aligns with the trajectory of the entire path, resulting in a smoother optimal path. This prevents the mobile robot from interfering with another global path while moving along the optimal path. This prevents one mobile robot from interfering with another mobile robot moving between a different initial node and a different target node.

[0020] Furthermore, S9 specifically includes the following steps:

[0021] S9.1. Evaluate the speed of different local paths using a speed evaluation function.

[0022] The speed evaluation function is:

[0023]

[0024] a is the steering wheel angle deviation constant of the mobile robot; β is the angle error constant of the mobile robot; γ is the deceleration constant; δ is the acceleration constant; η is the target following error constant.

[0025] delta_cost(v,w) is the cost of the deviation between the optimal steering wheel angle and the current vehicle body angle; dis_angle(v,w) is the cost of the deviation between the optimal vehicle body angle and the current vehicle body angle; traj.xv is the linear velocity when tracing the path; diff_delta is the cost of the deviation between the optimal steering wheel angle and the current steering wheel angle; dis_goal(v,w) is the target node evaluation function; vel(v,w) is the velocity evaluation function; dis_goal(v,w) is the penalty cost for exceeding the end point.

[0026] S9.2. Select the local path with the lowest evaluation score as the optimal path.

[0027] When evaluating the speed, the above method considers the cost value of the deviation between the optimal steering wheel orientation angle and the current orientation angle of the vehicle body, the cost value of the deviation between the optimal orientation angle of the vehicle body and the current orientation angle of the vehicle body, and the cost value of the deviation between the optimal orientation angle of the steering wheel and the current orientation angle of the steering wheel; the smaller the cost value, the smaller the moving distance of the mobile robot relative to the global path, and the smaller the cost value, the higher the fit between the local path and the entire path.

[0028] Furthermore, delta_cost(v,w) is calculated by the following formula;

[0029]

[0030]

[0031] θ bw is the optimal steering angle of the steering wheel; θ p is the angle of entering the next node p on the local path; θ t is the navigation angle of the mobile robot at time t; θ r is the current navigation angle of the mobile robot; θ pr is the angle between the next node p in the local path and the current coordinates of the mobile robot; the average value of delta_cost(v,w) at different times is used as the cost value of the deviation between the optimal steering wheel orientation angle and the current orientation angle of the vehicle body.

[0032] The above method calculates the average value, and the cost value of the deviation between the optimal steering wheel heading angle and the current steering angle of the vehicle body is highly accurate.

[0033] Furthermore, dis_angle(v,w) is calculated by the following formula;

[0034]

[0035]

[0036] yp is the y coordinate of the next node p; t is the y coordinate of the mobile robot at time t; x p is the x coordinate of the next node p; x t is the x-coordinate of the mobile robot at time t; the average value of dis_angle(v,w) at different times is used as the cost value of the deviation between the optimal orientation angle of the vehicle body and the current orientation angle of the vehicle body.

[0037] The above method calculates the average value, and the cost value of the deviation between the optimal vehicle body orientation angle and the current vehicle body orientation angle is highly accurate.

[0038] Furthermore, diff_delta is calculated by the following formula; diff_delta=θ bw -θ w； θ w is the current steering wheel heading angle.

[0039] Furthermore, after S9, S10 is further included. S10: when the mobile robot is at the end node of the local path, it is determined whether the node with the shortest distance to the mobile robot on the global path is the target node; if not, dis_goal(v, ω) is 0; if so, dis_goal(v, ω)=L*cosθ L ; L is the distance between the local path end node and the target node; θ L It is the angle between the line connecting the end node of the local path and the target node relative to the direction of the preferred path line.

[0040] In the above method, when the local path end node is inconsistent with the target node, it means that the target node is located in front of the local path end node, and at this time, it does not interfere with the mobile robot moving to the local path end node; when the local path end node is consistent with the target node, if the mobile robot moves in front of the local path end node, it will cause the mobile robot to exceed the target node, thereby passing L and θ L The distance LA that the mobile robot exceeds the target node is calculated so that the mobile robot can return to the target node accurately.

[0041] Furthermore, S1 also includes a preset moving speed range of the mobile robot;

[0042] The above method presets the maximum speed range and minimum speed range of the mobile robot; it can be adjusted according to different usage scenarios to meet different usage needs.

[0043] Furthermore, S1 also includes a preset maximum acceleration of the mobile robot;

[0044] v c is the current linear velocity of the mobile robot, w c is the current angular velocity of the mobile robot; is the maximum linear velocity of the mobile robot; is the minimum linear velocity of the mobile robot; is the maximum angular velocity of the mobile robot; is the maximum angular velocity of the mobile robot.

[0045] Furthermore, S1 also includes a preset maximum deceleration of the mobile robot;

[0046] is the shortest distance between the mobile robot and the obstacle. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 Flowchart of the present invention.

[0048] Figure 2 Schematic diagram of a mobile robot approaching the next node on a local path.

[0049] Figure 3 Schematic diagram of a mobile robot approaching the target node. DETAILED DESCRIPTION

[0050] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0051] like Figure 1-3 As shown in FIG, a global path planning method for a forklift based on a novel speed evaluation space of DWA includes the following steps:

[0052] S1. Preset a map, build a global coordinate system in the map, and build a robot coordinate system with the midpoint of the mobile robot as the origin; each coordinate point in the global coordinate system is a node.

[0053] The initial node and target node are preset, the initial node is the starting point of the mobile robot, and the target node is the end point of the mobile robot.

[0054] S2. Generate a global path between the initial node and the target node. The global path is the shortest path between the initial node and the target node.

[0055] S3. The mobile robot moves along the global path 1, approaching the target node 11 from the initial node.

[0056] S4. Determine whether the mobile robot encounters an obstacle 10 on the global path. If so, proceed to S5; if not, continue moving along the global path.

[0057] S5. Calculate the cost between each node on the global path and the obstacle, and set the node with a cost greater than zero to the obstacle as a temporary stop node; the mobile robot stops moving after moving to the temporary stop node.

[0058] S6. Preset a pause time, start timing the pause time, detect obstacles, and repeatedly calculate the cost value between the temporary stop node and the obstacle. If the cost value is equal to zero, it means that the obstacle has disappeared, and then proceed to S7; if the cost value between the temporary stop node and the obstacle is still greater than zero after the pause time ends, it means that the obstacle still exists, and then proceed to S8.

[0059] S7, the mobile robot continues to move along the global path;

[0060] S8. Generate one or more local paths 2 that bypass obstacle 1. In this embodiment, the local paths are generated using the A* algorithm.

[0061] S9. Evaluate the speed of more than one local path and select the local path closest to the global path as the optimal path.

[0062] S10. The mobile robot moves along the optimal path toward the target node. Determine whether the node with the shortest distance to the mobile robot on the global path 1 is the target node 11 when the mobile robot is at the end node K of the local path. If not, dis_goal(v, ω) is 0. If so, dis_goal(v, ω) = L*cosθ L ; L is the distance between the local path end node K and the target node 11; θ L is the angle between the line connecting the local path end node and the target node relative to the direction of the preferred path line. When the local path end node is inconsistent with the target node, it means that the target node is in front of the local path end node, and it does not interfere with the movement of the mobile robot to the local path end node. When the local path end node is consistent with the target node, if the mobile robot moves in front of the local path end node, it will cause the mobile robot to overtake the target node, thus passing L and θ. L Calculate the distance that the mobile robot exceeds the target node so that the mobile robot can return to the target node accurately.

[0063] S9 specifically includes the following steps:

[0064] S9.1. Evaluate the speed of different local paths using a speed evaluation function.

[0065] The speed evaluation function is:

[0066]

[0067] a is a rudder angle deviation constant of the mobile robot; β is an angle error constant of the mobile robot; γ is a deceleration constant; δ is an acceleration constant; η is a target following error constant.

[0068] delta_cost(v,w) is a cost value of a deviation of the rudder optimal heading angle from the current heading angle of the vehicle; dis_angle(v,w) is a cost value of a deviation of the vehicle optimal heading angle from the current heading angle of the vehicle; traj.xv is a linear velocity at the time of the tracking path; diff_delta is a cost value of a deviation of the rudder optimal heading angle from the current heading angle of the rudder; dis_goal(v,w) is a goal node evaluation function; vel(v,w) is a velocity evaluation function; dis_goal(v,w) is a penalty cost value for exceeding the end point.

[0069] S9.2, selecting a local path having the lowest evaluation score as the optimal path.

[0070] When the velocity is evaluated, the cost value of the deviation of the rudder optimal heading angle from the current heading angle of the vehicle, the cost value of the deviation of the vehicle optimal heading angle from the current heading angle of the vehicle, and the cost value of the deviation of the rudder optimal heading angle from the current heading angle of the rudder are considered; the smaller the cost value is, the smaller the moving distance of the mobile robot from the relative global path is, and the smaller the cost value is, the higher the fitting degree of the local path to the global path is.

[0071] delta_cost(v,w) is calculated by the following equation;

[0072] delta_cost(v,w)=θ bw -θ t .

[0073] θ bw is the rudder optimal heading angle; θ p is an angle for entering a next node p of the local path; θ t is a vehicle navigation angle of the mobile robot at the time t; θ r is a current navigation angle of the mobile robot; θ pr is an angle between the next node p of the local path and the current coordinate of the mobile robot; an average value of the delta_cost(v,w) values at different times is used as the cost value of the deviation of the rudder optimal heading angle from the current heading angle of the vehicle. By calculating the average value, the rudder optimal

[0074] dis_angle(v,w) is calculated by the following equation;

[0075] dis_angle(v,w)=|θ p -θ t |

[0076]

[0077] y p is the y coordinate of the next node p; t is the y coordinate of the mobile robot at time t; x p is the x coordinate of the next node p; x t is the x-coordinate of the mobile robot at time t. The average of the dis_angle(v,w) values ​​at different times is used as the cost value for the deviation between the optimal vehicle orientation angle and the current vehicle orientation angle. By calculating the average value, the cost value for the deviation between the optimal vehicle orientation angle and the current vehicle orientation angle is highly accurate.

[0078] diff_delta is calculated by the following formula; diff_delta=θ bw -θ w； θ w is the current steering wheel heading angle.

[0079] S1 also includes the preset moving robot's moving speed range, maximum acceleration and maximum deceleration speed.

[0080] Moving speed range v1={(v,w) | v∈[v min ,v max ],w∈[w min ,w max The maximum and minimum speed ranges of the mobile robot are preset; they can be adjusted according to different usage scenarios to meet different usage needs.

[0081] Maximum acceleration v c is the current linear velocity of the mobile robot, w c is the current angular velocity of the mobile robot; is the maximum linear velocity of the mobile robot; is the minimum linear velocity of the mobile robot; is the maximum angular velocity of the mobile robot; is the maximum angular velocity of the mobile robot.

[0082] Maximum deceleration dist(v,w) is the shortest distance between the mobile robot and the obstacle.

[0083] In this embodiment, the mobile robot is a forklift-type mobile robot. The working principle of the present invention is to first generate a global path between an initial node and a target node, and then determine the shortest path between the initial node and the target node, so that the mobile robot can quickly approach the target node. As the mobile robot approaches the target node, it scans for obstacles in real time. When an obstacle blocks the mobile robot's progress, it generates a local path to avoid the obstacle. Simultaneously, the local path that is closest to the global path is selected as the optimal path. This ensures that the trajectory of the optimal path aligns with the trajectory of the entire path, resulting in a smoother optimal path and preventing the mobile robot from interfering with another global path while moving along the optimal path. This prevents one mobile robot from interfering with another mobile robot moving between a different initial node and a different target node.

[0084] The working principle of the present invention is as follows: first, a global path between the initial node and the target node is generated, and the shortest path between the initial node and the target node is determined, so that the mobile robot can quickly approach the target node; obstacles are scanned in real time during the process of the mobile robot approaching the target node; but after the obstacle is identified, a temporary stop node is calculated based on the cost value, and the mobile robot moves to the temporary stop node to wait; the position of the obstacle is detected in real time during the pause time; when the obstacle moves away from the mobile robot, the cost value between the mobile robot and the obstacle decreases, and thus does not interfere with the mobile robot, so that the mobile robot continues to move toward the target node without replanning the path, with good smoothness and simple calculation; and after the pause time, if the cost value between the mobile robot and the obstacle does not change, it means that the obstacle will interfere with the movement of the mobile robot, so the obstacle is avoided. By setting the pause time, whether the obstacle will interfere with the movement of the mobile robot is judged within a time period, reducing the number of times the mobile robot replans the path, with simple calculation and good smoothness.

[0085] When an obstacle blocks the mobile robot's progress, a local path is generated to avoid it. The local path that is closest to the global path is selected as the optimal path. This ensures that the trajectory of the optimal path aligns with the trajectory of the entire path, resulting in a smoother optimal path. This prevents the mobile robot from interfering with another global path while moving along the optimal path. This prevents one mobile robot from interfering with another mobile robot moving between a different initial node and a different target node.

Claims

1. A forklift global path planning method based on the improved speed evaluation space of DWA, characterized by: The following steps are involved: S1. Preset a map, build a global coordinate system in the map, and build a robot coordinate system with the midpoint of the mobile robot as the origin; each coordinate point in the global coordinate system is a node; The initial node and target node are preset, the initial node is the starting point of the mobile robot, and the target node is the end point of the mobile robot; S2. Generate a global path between the initial node and the target node. The global path is the shortest path between the initial node and the target node. S3, the mobile robot moves along the global path, approaching the target node from the initial node; S4, determining whether the mobile robot encounters an obstacle on the global path, if so, proceeding to S5; if not, continuing to move along the global path; S5. Calculate the cost between each node on the global path and the obstacle, and set the node with a cost greater than zero to the obstacle as a temporary stop node; the mobile robot stops moving after moving to the temporary stop node; S6. A pause time is preset, and the pause time starts counting. Obstacles are detected and the cost between the temporary stop node and the obstacle is repeatedly calculated. If the cost is equal to zero, it indicates that the obstacle has disappeared, and then S7 is performed. If the cost between the temporary stop node and the obstacle is still greater than zero after the pause time ends, it indicates that the obstacle still exists, and then S8 is performed. S7, the mobile robot continues to move along the global path; S8. Generate one or more local paths that circumvent obstacles; S9, evaluating the speed of more than one local path, and selecting the local path closest to the global path as the optimal path; S10, the mobile robot moves along the optimal path to approach the target node; S9 specifically includes the following steps; S9.

1. Evaluate the speed of different local paths using a speed evaluation function; The speed evaluation function is: a is the steering wheel angle deviation constant of the mobile robot; β is the angle error constant of the mobile robot; γ is the deceleration constant; δ is the acceleration constant; η is the target following error constant; delta_cost(v,w) is the cost of the deviation between the optimal steering wheel angle and the current vehicle body angle; dis_angle(v,w) is the cost of the deviation between the optimal vehicle body angle and the current vehicle body angle; traj.xv is the linear velocity during the path tracking; diff_delta is the cost of the deviation between the optimal steering wheel angle and the current steering wheel angle; dis_goal(v,w) is the target node evaluation function; vel(v,w) is the velocity evaluation function; dis_goal(v,w) is the penalty cost for exceeding the end point; S9.

2. Select the local path with the lowest evaluation score as the optimal path.

2. The forklift global path planning method based on the improved speed evaluation space of DWA according to claim 1 is characterized by: delta_cost(v,w) is calculated by the following formula; delta_cost(v,w)=θ bw -θ t ; i bw =arctan[(2θ p -θ t ) / (θ pr -θ r )]; θ bw is the optimal heading angle of the steering wheel; θ p is the angle of entering the next node p on the local path; θ t is the navigation angle of the mobile robot at time t; θ r is the current navigation angle of the mobile robot; θ pr is the angle between the next node p in the local path and the current coordinates of the mobile robot; the average value of delta_cost(v,w) at different times is used as the cost value of the deviation between the optimal steering wheel orientation angle and the current orientation angle of the vehicle body.

3. The forklift global path planning method based on the improved speed evaluation space of DWA according to claim 2, characterized in that: dis_angle(v,w) is calculated by the following formula; dis_angle(v,w)=|θ p -θ t |; θ p =arctan[(and p -and t ) / (x p -x t )]; y p is the y coordinate of the next node p; t is the y coordinate of the mobile robot at time t; x p is the x coordinate of the next node p; x t is the x-coordinate of the mobile robot at time t.

4. The forklift global path planning method based on the improved speed evaluation space of DWA according to claim 3, characterized in that: diff_delta is calculated by the following formula; diff_delta=θ bw -θ w； θ w is the current steering wheel heading angle.

5. The forklift global path planning method based on the improved speed evaluation space of DWA according to claim 3 is characterized by: S9 also includes S10, S10, when the mobile robot is at the end node of the local path, it is determined whether the node with the shortest distance to the mobile robot on the global path is the target node; if not, dis_goal(v, ω) is 0; if so, dis_goal(v, ω)=L*cosθ L ; L is the distance between the local path end node and the target node; θ L It is the angle between the line connecting the end node of the local path and the target node relative to the direction of the preferred path line.

6. The forklift global path planning method based on the improved speed evaluation space of DWA according to claim 1 is characterized by: S1 also includes the preset mobile robot's moving speed range; v1={(v,w) | v∈[v min ,v max ],w∈[w min ,w max ]},v max is the maximum speed range of the mobile robot and v min Minimum speed range.

7. The forklift global path planning method based on the improved speed evaluation space of DWA according to claim 6 is characterized in that: S1 also includes the maximum acceleration of the preset mobile robot; v c is the current linear velocity of the mobile robot, w c is the current angular velocity of the mobile robot; is the maximum linear velocity of the mobile robot; is the minimum linear velocity of the mobile robot; is the maximum angular velocity of the mobile robot; is the maximum angular velocity of the mobile robot.

8. The forklift global path planning method based on the improved speed evaluation space of DWA according to claim 7 is characterized in that: S1 also includes the maximum deceleration of the preset mobile robot; dist(v,w) is the shortest distance between the mobile robot and the obstacle.

Images