Novel speed evaluation space forklift local path planning method based on DWA

Abstract

According to the forklift global path planning method of the novel speed evaluation space based on the DWA, whether an obstacle interferes with movement of a mobile robot or not is judged within a time period, the number of times of re-planning a path by the mobile robot is reduced, the working efficiency is improved, and the smoothness is good; meanwhile, when the obstacle blocks the moving robot from advancing, a local path is generated for obstacle avoidance. Meanwhile, selecting a local path closest to the global path as an optimal path; in this way, the track of the optimal path fits the tracks of all the paths, the smoothness of the optimal path is good, and the situation that another global path is interfered when the mobile robot moves along the optimal path is avoided; in this way, one mobile robot does not interfere with the mobile robot moving between another different initial node and different target nodes.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a forklift local path planning method based on a novel speed evaluation space of DWA. Background Art

[0002] As a typical mobile robot, forklift is widely used in transportation and production. It has the advantage of being able to work for a long time and with high efficiency. However, during the operation of the forklift, the planning of the forklift's moving path will determine the forklift's working efficiency, thus highlighting the importance of path planning methods.

[0003] For example, a patent document with application number 202211581793.1 in China and publication date 2023.2.24 discloses a global movement method for a mobile robot. In the process of the initial node approaching the target node, the node with the smallest cost value is searched, and then the mobile robot moves before the node with the smallest cost value; the moving distance and moving time of the mobile robot are short; at the same time, for known obstacles, the cost values ​​of nodes other than the obstacle nodes are calculated; the mobile robot avoids obstacle nodes during movement to avoid known obstacles; for newly appearing obstacles, a local path is planned in the process of the mobile robot approaching the newly appearing obstacles to avoid unknown obstacles.

[0004] In this moving method, when avoiding a newly appearing obstacle, the mobile robot deviates from the set trajectory to avoid the obstacle, and then plans the shortest path between the current node and the end point of the path after avoiding the obstacle; the speed of the mobile robot is not considered when planning the path, and the smoothness of the walking path of the mobile robot is poor; at the same time, it is easy to interfere with mobile robots on other paths. Summary of the invention

[0005] The present invention provides a forklift global path planning method based on a novel speed evaluation space of DWA, which determines whether an obstacle will interfere with the movement of a mobile robot within a time period, reduces the number of times the mobile robot replans the path, improves work efficiency, and has good smoothness; at the same time, when planning a local path, it is close to the global path and has good smoothness; at the same time, it avoids interfering with mobile robots on other paths.

[0006] To achieve the above object, the technical solution of the present invention is: a forklift global path planning method based on a new speed evaluation space of DWA, comprising the following steps: S1. Preset a map, construct a global coordinate system in the map, and construct the robot coordinate system with the midpoint of the mobile robot as the origin; each coordinate point in the global coordinate system is a node.

[0007] The initial node and the 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.

[0008] 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.

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

[0010] 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.

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

[0012] 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 disappears, and then proceed to S7; if after the pause time ends, the cost value between the temporary stop node and the obstacle is still greater than zero, it means that the obstacle still exists, and then proceed to S8.

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

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

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

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

[0017] The above method first generates a global path between the initial node and the target node, determines the shortest path between the initial node and the target node, and enables the mobile robot to quickly approach the target node; scans obstacles in real time while the mobile robot approaches the target node; after identifying the obstacle, calculates the temporary stop node based on the cost value, and the mobile robot moves to the temporary stop node to wait; detects the position of the obstacle in real time during the pause time; when the obstacle is far away from the mobile robot, the cost value between the mobile robot and the obstacle decreases, and then will not interfere with the mobile robot, so that the mobile robot continues to move to the target node without replanning the path, with good smoothness and simple calculation; and after the pause time ends, 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 as to avoid the obstacle. By setting the pause time, it is judged whether the obstacle will interfere with the movement of the mobile robot within a time period, reducing the number of times the mobile robot replans the path, with simple calculation and good smoothness.

[0018] When an obstacle blocks the mobile robot's progress, a local path is generated to avoid the obstacle. At the same time, the local path closest to the global path is selected as the optimal path; in this way, the trajectory of the optimal path fits the trajectory of the entire path, and the optimal path has good smoothness, which avoids the mobile robot from interfering with another global path when moving along the optimal path; in this way, a mobile robot will not interfere with another mobile robot moving between different initial nodes and different target nodes.

[0019] Further, S9 specifically includes the following steps: S9.1. Evaluate the speed of different local paths using a speed evaluation function.

[0020] The speed evaluation function is:

[0021] 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.

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

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

[0024] The above method, when evaluating the speed, considers the cost value of the deviation between the optimal orientation angle of the steering wheel 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 thus the smaller the cost value, the higher the fit between the local path and the entire path.

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

[0026]

[0027] θ 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 taken as the cost value of the deviation between the optimal steering angle of the steering wheel and the current steering angle of the vehicle body.

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

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

[0030]

[0031] 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; t is the x-coordinate of the mobile robot at time t; the average value of dis_angle(v,w) at different times is taken 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.

[0032] In the above method, by calculating the average value, the cost value of the deviation between the optimal orientation angle of the vehicle body and the current orientation angle of the vehicle body has high accuracy.

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

[0034] Furthermore, after S9, S10 is also 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 straight line.

[0035] 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 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 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 accurately return to the target node.

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

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

[0038] Furthermore, S1 also includes a preset maximum acceleration of the 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.

[0039] Furthermore, S1 also includes a preset maximum deceleration of the mobile robot; is the shortest distance between the mobile robot and the obstacle. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 It is a flow chart of the present invention.

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

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

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

[0044] like Figure 1-3 As shown in the figure, a forklift global path planning method based on a new speed evaluation space of DWA includes the following steps: S1. Preset a map, construct a global coordinate system in the map, and construct the robot coordinate system with the midpoint of the mobile robot as the origin; each coordinate point in the global coordinate system is a node.

[0045] The initial node and the 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.

[0046] 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.

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

[0048] S4, determining whether the mobile robot encounters an obstacle 10 on the global path, if so, proceeding to S5; if not, continuing to move along the global path.

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

[0050] 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 disappears, and then proceed to S7; if after the pause time ends, the cost value between the temporary stop node and the obstacle is still greater than zero, it means that the obstacle still exists, and then proceed to S8.

[0051] S7, the mobile robot continues to move along the global path; S8. Generate one or more local paths 2 that bypass obstacle 1. In this embodiment, the local paths are generated by using the A* algorithm.

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

[0053] S10, the mobile robot moves along the optimal path to approach the target node. When the mobile robot is at the local path end node K, determine whether the node with the shortest distance to the mobile robot on the global path 1 is the target node 11; if not, dis_goal(v, ω) is 0; if yes, 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 straight line. 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 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 exceed the target node, thereby passing L and θ L Calculate the distance that the mobile robot exceeds the target node so that the mobile robot can accurately return to the target node.

[0054] S9 specifically includes the following steps: S9.1. Evaluate the speed of different local paths using a speed evaluation function.

[0055] The speed evaluation function is:

[0056] 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.

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

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

[0059] When evaluating the speed, the cost value of the deviation between the optimal orientation angle of the steering wheel 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 are considered; 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.

[0060] delta_cost(v,w) is calculated by the following formula; delta_cost(v,w)=θ bw -θ t .

[0061] θ 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 angle of the steering wheel and the current steering angle of the vehicle body. dis_angle(v,w) is calculated by the following formula; dis_angle(v,w)=|θ p -θ t |.

[0062]

[0063] 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; 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. By calculating the average value, the cost value of the deviation between the optimal orientation angle of the vehicle body and the current orientation angle of the vehicle body is highly accurate.

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

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

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

[0067] 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.

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

[0069] 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 the initial node and the target node, determine the shortest path between the initial node and the target node, so that the mobile robot can quickly approach the target node; scan obstacles in real time during the process of the mobile robot approaching the target node; when an obstacle blocks the mobile robot from moving forward, a local path is generated to avoid the obstacle. At the same time, the local path closest to the global path is selected as the optimal path; in this way, the trajectory of the optimal path fits the trajectory of all paths, and the optimal path has good smoothness, which avoids the mobile robot from interfering with another global path when moving along the optimal path; in this way, a mobile robot will not interfere with another mobile robot moving between different initial nodes and different target nodes.

[0070] The working principle of the present invention is as follows: first, a global path between an initial node and a 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; after the obstacle is identified, a temporary stop node is calculated according to 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 is far away from the mobile robot, the cost value between the mobile robot and the obstacle is reduced, and then it will not interfere with the mobile robot, so that the mobile robot continues to move to the target node without re-planning the path, with good smoothness and simple calculation; and after the pause time ends, 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 as to avoid the obstacle. By setting the pause time, it is judged whether the obstacle will interfere with the movement of the mobile robot within a time period, and the number of times the mobile robot re-plans the path is reduced, and the calculation is simple and smooth.

[0071] When an obstacle blocks the mobile robot's progress, a local path is generated to avoid the obstacle. At the same time, the local path closest to the global path is selected as the optimal path; in this way, the trajectory of the optimal path fits the trajectory of the entire path, and the optimal path has good smoothness, which avoids the mobile robot from interfering with another global path when moving along the optimal path; in this way, a mobile robot will not interfere with another mobile robot moving between different initial nodes and different target nodes.

Claims

1. A forklift global path planning method based on a new speed evaluation space of DWA, characterized by: The following steps are involved: S1. Preset a map, construct a global coordinate system in the map, and construct the 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 the 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, determine whether the mobile robot encounters an obstacle on the global path, if so, proceed to S5; if not, continue to move along the global path; S5, calculating the cost value between each node on the global path and the obstacle, and setting the node whose cost value between the node and the obstacle is greater than zero as a temporary stop node; the mobile robot stops moving after moving to the temporary stop node; 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 disappears, 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; S7, the mobile robot continues to move along the global path; S8, generating one or more local paths that bypass 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 toward the target node.

2. The forklift global path planning method based on the novel speed evaluation space of DWA according to claim 1 is characterized by: 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 value of the deviation between the optimal steering angle of the steering wheel and the current steering angle of the vehicle body; dis_angle(v,w) is the cost value of the deviation between the optimal steering angle of the vehicle body and the current steering angle of the vehicle body; traj.xv is the linear speed when tracing the path; diff_delta is the cost value of the deviation between the optimal steering angle of the steering wheel and the current steering angle of the steering wheel; dis_goal(v,w) is the target node evaluation function; vel(v,w) is the speed evaluation function; dis_goal(v,w) is the penalty cost value for exceeding the end point; S9.

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

3. The forklift global path planning method based on the novel 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 best 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 angle of the steering wheel and the current steering angle of the vehicle body.

4. The forklift global path planning method based on the novel speed evaluation space of DWA according to claim 3 is characterized by: 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; t is the x-coordinate of the mobile robot at time t.

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

6. The forklift global path planning method based on the novel speed evaluation space of DWA according to claim 4 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 local path end node and the target node relative to the direction of the preferred path straight line.

7. The forklift global path planning method based on the new 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 ]}.

8. The forklift global path planning method based on the new speed evaluation space of DWA according to claim 7 is characterized by: 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.

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

Images