A local path planning method for mobile robots based on DWA

Abstract

The application provides a local path planning method for a mobile robot based on DWA, in which nodes with large angle differences between two nodes are deleted, and then new nodes are supplemented between the two nodes; the angle deviation between adjacent nodes is reduced, the inflection points of the path are reduced, the smoothness of the movement of the mobile robot is improved, a forward-looking point is set so that the mobile robot deviating from the optimized path enters the optimized path, the forward-looking point prolongs the distance of the mobile robot entering the optimized path, and the smoothness of the movement of the mobile robot is improved.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and more specifically to a local path planning method for mobile robots based on DWA. Background Technology

[0002] A mobile robot is an intelligently controlled device that moves to perform various tasks. With the continuous development of artificial intelligence, mobile robots are gradually becoming able to replace humans in performing some jobs. Compared to humans, robots have the advantages of working for longer periods and with higher efficiency. Mobile robots, especially service-oriented mobile robots, such as robotic vacuum cleaners and delivery robots, are becoming increasingly common in daily life.

[0003] In the field of mobile robot technology, planning and control is an important research area, and commonly used planning algorithms include A* algorithm and DWA (Dynamic Window Method) algorithm.

[0004] For example, Chinese patent application number 202211581793.1, published on February 24, 2023, discloses a global movement method for a mobile robot. During the process of approaching the target node from the initial node, the method searches for the node with the lowest cost value, and then the mobile robot moves before the node with the lowest cost value. The mobile robot has a short movement distance and short movement time. Simultaneously, for known obstacles, the method calculates the cost value of nodes other than the obstacle nodes, enabling the mobile robot to avoid obstacle nodes during movement, thus achieving obstacle avoidance for known obstacles. For newly appearing obstacles, during the process of the mobile robot approaching the newly appearing obstacle, the method plans an obstacle avoidance path, thus achieving obstacle avoidance for unknown obstacles.

[0005] In this movement method, the A* algorithm is used to select the node with the highest priority from the search area as the next node to be traversed. However, since the A* algorithm selects the node with the lowest cost, an uneven path will appear, and the deflection angle between adjacent nodes will be too large. This will cause the turning angle of the mobile robot to increase, making it difficult for the mobile robot to move and thus affecting the guidance efficiency. Summary of the Invention

[0006] This invention provides a local path planning method for mobile robots based on DWA, which reduces the deflection angle between adjacent nodes, reduces the number of inflection points on the path, improves the smoothness of the mobile robot's movement, sets look-ahead points to allow the mobile robot that deviates from the optimized path to enter the optimized path, and extends the distance for the mobile robot to enter the optimized path, thereby improving the smoothness of the mobile robot's movement.

[0007] To achieve the above objectives, the technical solution of the present invention is: a local path planning method for mobile robots based on DWA, comprising the following steps:

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

[0009] An initial node and a 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.

[0010] S2, an original path between the initial node and the target node is generated, and the original path is the shortest path between the initial node and the target node.

[0011] S3, an angle difference threshold is preset; any three adjacent nodes are selected, a first included angle between one node and another node is calculated, a second included angle between the other node and another node is calculated, and an angle difference between the first included angle and the second included angle is calculated.

[0012] S4, whether the current angle difference is greater than the angle difference threshold is judged, if yes, S5 is performed.

[0013] S5, the middle node of the three adjacent nodes is deleted in the original path.

[0014] S6, the midpoint between the remaining two nodes is calculated.

[0015] S7, the midpoint is updated as a new node in the original path to form an optimized path.

[0016] S8, a projection point of the mobile robot on the optimized path is confirmed, and the projection point is the node closest to the center of the mobile robot on the optimized path.

[0017] S9, in the forward direction of the optimized path, the next node in front of the projection point is set as a lookahead point (x p , y p ).

[0018] S10, a movement path between the mobile robot and the lookahead point is generated through the current speed of the mobile robot, the deviation angle between the movement direction of the current movement trajectory of the mobile robot under the current speed and the lookahead point.

[0019] In the above method, the node with an angle difference between two nodes exceeding a certain threshold is deleted, and then a new node is supplemented between the two nodes; the new node is the midpoint between the two nodes, so that the three adjacent nodes are connected in a straight line; the deviation angle between adjacent nodes is reduced, the inflection point of the path is reduced, the smoothness of the movement of the mobile robot is improved, the turning angle of the mobile robot on the optimized path is small, and the movement of the mobile robot is facilitated.

[0020] Since the updated optimization path is inconsistent with the original path, the mobile robot deviates from the optimization path; thus, a path between the mobile robot and the optimization path needs to be planned; the shortest distance between the optimization path and the mobile robot is confirmed, the distance of the mobile robot moving to the optimization path is limited, and the mobile robot is prevented from moving beyond the optimization path; the position of the projection point is used to confirm the forward-looking point, the distance of the mobile robot entering the optimization path is extended, the turning radius of the mobile robot directly entering the optimization path is reduced, and the smoothness of the movement of the mobile robot is improved.

[0021] Further, S10 specifically comprises:

[0022] The speed range of the mobile robot at time t is set as Vt∈[Vmin, Vmax], Wt∈[Wmin, Wmax];

[0023] The movement path between the mobile robot and the forward-looking point is calculated through the formula G(v, ω)=α*dis_angle(v,ω)+β*dis_local(v,ω)+γ*vel(v,ω)+c*dis_goal(v,ω).

[0024] α, β, γ, and c are weight coefficients; dis_angle(v,ω) is the value of the angle difference between the coordinate of the mobile robot at time t in the movement path and the angle of the vehicle body.

[0025] dis_local(v,ω) is the value of the angle difference between the current coordinate of the mobile robot and the angle of the vehicle body; vel(v,ω) is the value of the current speed of the mobile robot; and dis_goal(v,ω) is the penalty value of the mobile robot exceeding the target node.

[0026] The above method, when planning the movement path, is affected by the speed, and if the angle difference between the current position coordinate of the mobile robot and the angle of the vehicle body is large, the value will also increase as the speed increases; then, the speed is constrained according to the angle difference between the current position coordinate of the mobile robot and the angle of the vehicle body, and the angle difference between the next position coordinate of the mobile robot at time t and the angle of the vehicle body at the current speed; if the angle difference of the current position is less than the angle difference at time t, the cost of the mobile robot moving to the optimization path increases, and the speed needs to be reduced; if the angle difference of the current position is greater than the angle difference at time t, the cost of the mobile robot moving to the optimization path decreases, and the current speed can be maintained. Meanwhile, the penalty value of exceeding the target node is added in the formula to prevent the mobile robot from exceeding the target node.

[0027] Further, S10 further comprises the following steps:

[0028] S10.1 Calculate the coordinates (x, y) of the mobile robot at time t. t y t ) and the forward point (x p y p The third included angle θ between them pt The vehicle's orientation angle θ at time t is determined by the mobile robot. t The angle θ with the third pt Calculate dis_angle(v, ω).

[0029] S10.2 The specific calculation method of dis_local(v, ω) is as follows: calculate the current coordinates (x0, y0) and the look-ahead point (x0, y0) of the mobile robot. p y p The fourth included angle θ between them p0 Calculate the current orientation angle θ0 of the mobile robot and the included angle θ4. p0 The angle difference is determined by the included angle θ0 between the vehicle's heading angle and the fourth angle θ. p0 The angle difference and the linear velocity range Vt are used to calculate dis_local(v, ω). The calculation formula is dis_local(v, ω) = | arctan[(y p –y0) / (x p –x0)]- θ0|*Vt.

[0030] S10.3. When the mobile robot is at the end of its path, determine whether the node with the shortest distance to the mobile robot on the optimized path is the target node; if not, then dis_goal(v, ω) is 0; if so, then dis_goal(v, ω) = L*cosθ L L is the distance between the end node of the movement path and the target node; θ L The angle between the line connecting the end node of the moving path and the target node and the direction of the preferred path straight line.

[0031] In the above method, when the projection point and the target node are not the same, it means that the target node is in front of the projection point, and the robot's movement towards the lookahead point is not interfered with. When the projection point and the target node are the same, if the robot moves in front of the projection point, it will cause the robot to overtake the target node, thus affecting the movement of the robot via L and θ. L The distance the mobile robot exceeds the target node is calculated, enabling the mobile robot to accurately return to the target node.

[0032] Furthermore, in S2, the specific steps are as follows: calculate the F(n) value of all nodes using the formula F(n) = G(n) + H(n); G(n) is the cost value from the initial node to a node, H(n) is the estimated cost value from a node to the target node; F(n) is the evaluation function; and generate the original path using the node with the smallest F(n) value.

[0033] The above method uses the A* algorithm to select the node with the minimum cost between the initial node and the target node; the original path is generated by the initial node, the target node, and the node with the minimum cost, so that the distance between the initial node and the target node is short, and the mobile robot can quickly approach the target node.

[0034] Furthermore, through the formula Calculate the value of H(n); (x g y g (x) represents the coordinates of the next node. n y n ( ) represents the coordinates of a node.

[0035] The above method estimates the cost value using the distance formula between two points, and it has good accuracy.

[0036] Furthermore, S6 specifically refers to: using the formula xk = (x n + x m ) / 2; yk=(y n + y m ) / 2, calculate the coordinates of the midpoint (xk, yk), (xk, yk) n y n Let (x) be the coordinates of a node. m y m ( ) represents the coordinates of another node.

[0037] The above settings make it easy to calculate the midpoint coordinates. Attached Figure Description

[0038] Figure 1 This is a schematic diagram of the original path in this invention.

[0039] Figure 2 This is a schematic diagram of the optimized path in this invention.

[0040] Figure 3 This is a schematic diagram of the projection point, look-ahead point, and mobile robot in the optimized path of this invention.

[0041] Figure 4 This is a schematic diagram of the mobile robot exceeding the target node in this invention.

[0042] Figure 5 This is a flowchart of the present invention. Detailed Implementation

[0043] The application will be described in further detail below with reference to the drawings and specific embodiments.

[0044] As shown in the drawings, a DWA-based local path planning method for a mobile robot comprises the following steps: Figures 1-5

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

[0046] Presetting an initial node and a target node, the initial node being the starting point of the mobile robot and the target node being the end point of the mobile robot.

[0047] S2, generating an original path between the initial node and the target node, the original path being the shortest path between the initial node and the target node.

[0048] S3, presetting an angle difference threshold value; selecting any three adjacent nodes, calculating a first included angle between one node and another node, calculating a second included angle between the other node and yet another node, and calculating an angle difference between the first included angle and the second included angle.

[0049] S4, judging whether the current angle difference is greater than the angle difference threshold value, if yes, proceeding to S5, and if not, walking according to the original path.

[0050] S5, deleting the midpoint of the three adjacent nodes in the original path.

[0051] S6, calculating the midpoint between the remaining two nodes; in this embodiment, the coordinates of the midpoint (xk, yk) are calculated by the formula xk= (x n + x m ) / 2; yk= (y n + y m ) / 2, (x n , y n ) being the coordinates of one node and (x m , y m ) being the coordinates of another node.

[0052] S7, updating the midpoint to the original path as a new node to form an optimized path.

[0053] S8, confirming a projection point 1 of the mobile robot on the optimized path, the projection point 1 being the node on the optimized path closest to the center of the mobile robot.

[0054] S9, setting the next node in front of the projection point 1 as a look-ahead point 2 in the forward direction of the optimized path, the coordinates of the look-ahead point 2 being (x p ​, y p ).

[0055] S10, generate the moving path between the mobile robot and the look-ahead point 2 by the current speed of the mobile robot, the deviation angle between the moving direction of the current moving track of the mobile robot at the current speed and the look-ahead point 2.

[0056] The above method deletes the node with a large angle difference between two nodes, and then inserts a new node between the two nodes; the new node is the midpoint between the two nodes, so that the three adjacent nodes are connected in a straight line; the deviation angle between adjacent nodes is reduced, the inflection point of the path is reduced, and the smoothness of the mobile robot movement is improved. The steering angle of the mobile robot on the optimized path is small, which is convenient for the mobile robot to move.

[0057] Because the updated optimized path is inconsistent with the original path, the mobile robot will deviate from the optimized path; therefore, it is necessary to plan the path between the mobile robot and the optimized path; confirm the shortest distance between the optimized path and the mobile robot, limit the distance of the mobile robot moving to the optimized path, avoid the mobile robot moving to the optimized path beyond the optimized path; confirm the look-ahead point 2 by the position of the projection point 1, extend the distance of the mobile robot entering the optimized path, reduce the steering radius of the mobile robot directly entering the optimized path, and improve the smoothness of the mobile robot movement.

[0058] S10 is specifically: set the speed range of the mobile robot at time t, Vt∈[Vmin,Vmax], Wt∈[Wmin,Wmax].

[0059] In this embodiment, the linear speed range Vt of the mobile robot at time t is calculated by the formula Vt∈[V0-DvT,V0+AvT]; V0 is the current linear speed of the mobile robot; Dv is the maximum linear acceleration of the mobile robot; Av is the maximum linear deceleration of the mobile robot; T is the simulation period.

[0060] The linear angular velocity range Wt of the mobile robot at time t is calculated by the formula Wt∈[W0-Dw*T,W0+Aw*T]; W0 is the current angular velocity of the mobile robot; DwT is the maximum angular acceleration of the mobile robot; AwT is the minimum angular deceleration of the mobile robot; T is the simulation period.

[0061] The moving path between the mobile robot and the look-ahead point 2 is calculated by the formula G(v,ω)=α*dis_angle(v,ω)+β*dis_local(v,ω)+γ*vel(v,ω)+c*dis_goal(v,ω).

[0062] α, β, γ and c are custom weighting coefficients; dis_angle(v, ω) is the cost of the angle difference between the coordinates of the mobile robot at time t in the movement path and the angle of the look-ahead point 2 and the vehicle's facing angle.

[0063] dis_local(v, ω) is the cost of the angle difference between the current coordinates of the mobile robot and the angle of the look-ahead point 2 and the vehicle's orientation angle; vel(v, ω) is the cost of the current speed of the mobile robot; dis_goal(v, ω) is the cost of the penalty for the mobile robot exceeding the target node.

[0064] The above method, when planning the movement path, is affected by speed. If the angle difference between the robot's current position coordinates and the look-ahead point 2, and the vehicle's facing angle, is large, the cost will increase with increasing speed. Therefore, the speed is constrained based on the angle difference between the robot's current position coordinates and the look-ahead point 2, and the vehicle's facing angle, and the angle difference between the robot's next position coordinates at time t and the look-ahead point 2 at the current speed. If the angle difference at the current position is less than the angle difference at time t, the cost of the robot moving to the optimized path increases, and the speed needs to be reduced; if the angle difference at the current position is greater than the angle difference at time t, the cost of the robot moving to the optimized path decreases, and the current speed can be maintained. Simultaneously, a penalty for exceeding the target node is included in the formula to prevent the robot from exceeding the target node.

[0065] The calculation method for dis_angle(v, ω) includes: S10.1, calculating the coordinates (x, ω) of the mobile robot at time t. t y t ) and the coordinates of the lookahead point 2 (x p y p The third included angle θ between them pt The third included angle θ pt The angle can be calculated using the coordinates of the mobile robot obtained from the positioning system and the coordinates of the look-ahead point 2, using the angle calculation formula, as shown in the reference. Figure 3 As shown, 4 represents the mobile robot at time t, and the robot's orientation angle θ at time t is used to determine the position of the robot. t The angle θ with the third pt Calculate dis_angle(v, ω). The formula is dis_angle(v, ω) = |θ pt -θ t |=| arctan[(y p -y t ) / (x p -x t )]- θ t |

[0066] S10.2 Calculate the current coordinates (x0, y0) of the mobile robot and the coordinates of the look-ahead point 2 (x0, y0). p y p The fourth included angle θ between them p0 , refer to Figure 3 As shown in Figure 3, the current mobile robot is shown. The angle θ0 of the robot's current orientation and the included angle θ4 are calculated. p0 The angle difference, through θ0 and θ p0 The angle difference and the linear velocity range Vt are used to calculate dis_local(v, ω). The calculation formula is dis_local(v, ω) = | arctan[(y p –y0) / (x p –x0)]- θ0|*Vt.

[0067] S10.3. When the mobile robot is at the end of its path, determine whether the node with the shortest distance to the mobile robot on the optimized path is the target node; if not, then dis_goal(v, ω) is 0; if so, then dis_goal(v, ω) = L*cosθ L L is the distance between the end node of the movement path and the target node; θ L The angle between the line connecting the end node of the moving path and the target node and the direction of the preferred path straight line.

[0068] When projection point 1 is not aligned with the target node, it means the target node is in front of projection point 1, and the robot's movement towards lookahead point 2 is not interfered with. When projection point 1 is aligned with the target node, if the robot moves in front of projection point 1, it will cause the robot to overtake the target node, thus affecting the movement of the robot through L and θ. L Calculate the distance LA that the mobile robot exceeds the target node, so that the mobile robot can accurately return to the target node.

[0069] Specifically, in S2: The F(n) value of all nodes is calculated using the formula F(n) = G(n) + H(n); G(n) is the cost from the initial node to a given node, and H(n) is the estimated cost from a given node to the target node; F(n) is the evaluation function; the original path is generated using the node with the smallest F(n) value. Calculate the value of H(n); (x g y g (x) represents the coordinates of the next node. n y n) is the coordinate of a node. The estimated value of the distance between two points is obtained by the formula, which is accurate. The node with the minimum value of the distance between the initial node and the target node is selected by the A* algorithm. The original path is generated by the initial node, the target node and the node with the minimum value, so that the distance between the initial node and the target node is short, and the mobile robot can quickly approach the target node.

Claims

1. A DWA-based local path planning method for mobile robots, characterized in that: Includes the following steps: S1. Preset map, construct a global coordinate system on the map, and construct 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 system predefines an initial node and a target node. The initial node is the starting point of the mobile robot, and the target node is the ending point of the mobile robot. S2. Generate the original path between the initial node and the target node. The original path is the shortest path between the initial node and the target node. S3. Preset angle difference threshold; Select any three adjacent nodes, calculate the first included angle between one node and another node, calculate the second included angle between another node and yet another node, and calculate the angle difference between the first included angle and the second included angle. S4. Determine if the current angle difference is greater than the angle difference threshold. If so, proceed to S5. S5. Delete the intermediate node between three adjacent nodes in the original path; S6. Calculate the midpoint between the remaining two nodes; S7. Update the original path with the midpoint as a new node to form an optimized path; S8. Confirm the projection point of the mobile robot on the optimized path. The projection point is the node on the optimized path that is closest to the center of the mobile robot. S9, set the next node in front of the projection point as the look-ahead point (x p , y p ) in the advancing direction of the optimized path; S10. Generate the movement path between the mobile robot and the look-ahead point by using the current speed of the mobile robot, the deviation angle between the current movement trajectory of the mobile robot at the current speed and the look-ahead point.

2. The method according to claim 1, wherein: S10 includes: Set the velocity range of the mobile robot at time t, Vt∈[Vmin,Vmax], Wt∈[Wmin,Wmax], where Vmin is the minimum linear velocity, Vmax is the maximum linear velocity, Wmin is the minimum angular velocity, and Wmax is the maximum angular velocity; The movement path between the mobile robot and the lookout point is calculated using the formula G(v,ω)=α*dis_angle(v,ω)+β*dis_local(v,ω)+γ*vel(v,ω)+c*dis_goal(v,ω). α, β, γ and c are weighting coefficients; dis_angle(v, ω) is the cost of the angle difference between the coordinates of the mobile robot at time t in the movement path and the angle of the forward point and the vehicle's facing angle. dis_local(v, ω) is the cost of the angle difference between the current coordinates of the mobile robot and the angle of the look-ahead point and the vehicle's heading angle; vel(v, ω) is the cost of the current speed of the mobile robot; dis_goal(v, ω) is the cost of the penalty for the mobile robot exceeding the target node.

3. The method of claim 2, wherein: S10 also includes the following steps: S10.1 Calculate the coordinates (x, y) of the mobile robot at time t. t y t ) and the forward point (x p y p The third included angle θ between them pt The vehicle's orientation angle θ at time t is determined by the mobile robot. t The angle θ with the third pt Calculate dis_angle(v, ω); S10.2, the calculation method of dis_local(v, ω) is specifically: calculating the fourth included angle θ between the current coordinates (x0, y0) of the mobile robot and the look-ahead point (x p , p y ) , calculating the angle difference between the current body orientation angle θ0 of the mobile robot and the fourth included angle θ p0 , p0 , and calculating dis_local(v, ω) through the angle difference between θ0 and θ p0 and the linear velocity range Vt; the calculation formula is dis_local(v, ω) = |arctan[(y p – y0) / (x p – x0)]- θ0|*Vt; S10.3, judging whether the node with the shortest distance between the mobile robot and the node on the optimized path is the target node when the mobile robot is at the end node of the moving path; if not, dis_goal(v, ω) is 0; if yes, dis_goal(v, ω) = L*cosθ L ; L is the distance between the end node of the moving path and the target node; θ L is the included angle of the line connecting the end node of the moving path and the target node relative to the straight line direction of the optimized path.

4. The method of claim 1, wherein the method is based on DWA. Specifically in S2: the F(n) value of all nodes is calculated using the formula F(n) = G(n) + H(n); G(n) is the cost value from the initial node to a node, H(n) is the estimated cost value from a node to the target node; F(n) is the evaluation function; and the original path is generated through the node with the smallest F(n) value.

5. The local path planning method for a mobile robot based on DWA according to claim 4, characterized in that: The value of H(n) is calculated by the formula (x g , y g ) is the coordinate of the next node, and (x n , y n ) is the coordinate of a node.

6. The local path planning method for a mobile robot based on DWA according to claim 1, characterized in that: S6 is specifically: through the formula xk= (x n + x m ) / 2; yk= (y n + y m ) / 2, the coordinates of the midpoint (xk, yk) are calculated, (x n , y n ) is the coordinate of a node, (x m , y m ) is the coordinate of another node.

Images