Mobile robot local path planning method based on DWA

Abstract

The invention provides a mobile robot local path planning method based on a DWA, and the method comprises the steps: deleting a node with a large angle difference between two nodes, and adding a new node between the two nodes; the deflection angle between the adjacent nodes is reduced, the inflection points of the path are reduced, the moving smoothness of the mobile robot is improved, the prospective points are arranged to enable the mobile robot deviating from the optimized path to enter the optimized path, the distance of the mobile robot entering the optimized path is prolonged through the prospective points, and the moving smoothness of the mobile robot is improved.

Description

Technical Field

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

[0002] A mobile robot is a device that can be controlled to move and perform various tasks. With the continuous development of artificial intelligence, mobile robots have gradually been able to replace humans in some tasks. Compared with humans, robots have the advantage of being able to work for a long time and with high efficiency. Mobile robots, especially service-oriented mobile robots, such as sweeping robots and delivery robots, have gradually become common in daily life.

[0003] In the technical field of mobile robots, planning and control is an important research field. The more commonly used planning algorithms include the A* algorithm and the DWA (dynamic window method) algorithm.

[0004] 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, an obstacle avoidance path is planned in the process of the mobile robot approaching the newly appearing obstacles to avoid unknown obstacles.

[0005] In this mobile 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 smallest cost, an uneven path will appear and the deviation angle between adjacent nodes will be too large. This will cause the turning angle of the mobile robot to increase, making it inconvenient for the mobile robot to move, thereby affecting the guidance efficiency. Summary of the invention

[0006] The present invention provides a local path planning method for a mobile robot based on DWA, which reduces the deviation angle between adjacent nodes, reduces the turning points of the path, improves the smoothness of the movement of the mobile robot, sets a look-ahead point so that the mobile robot that deviates from the optimized path enters the optimized path, and the look-ahead point extends the distance for the mobile robot to enter the optimized path, thereby improving the smoothness of the movement of the mobile robot.

[0007] To achieve the above object, the technical solution of the present invention is: a local path planning method for a mobile robot based on 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.

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

[0009] S2. Generate an original path between the initial node and the target node, where the original path is the shortest path between the initial node and the target node.

[0010] S3, preset angle difference threshold; select any three adjacent nodes, calculate a first angle between one node and another node, calculate a second angle between another node and yet another node, and calculate the angle difference between the first angle and the second angle.

[0011] S4. Determine whether the current angle difference is greater than the angle difference threshold. If so, proceed to S5.

[0012] S5. Delete the middle nodes between three adjacent nodes in the original path.

[0013] S6. Calculate the midpoint between the remaining two nodes.

[0014] S7. Update the midpoint as a new node in the original path to form an optimized path.

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

[0016] S9, along the forward direction of the optimization path, set the next node in front of the projection point as the forward point (x p ,y p ).

[0017] S10, generating a moving path between the mobile robot and the forward point according to the current speed of the mobile robot and the deviation angle between the moving direction of the current moving trajectory of the mobile robot at the current speed and the forward point.

[0018] The above method deletes the nodes whose angle difference between two nodes exceeds a certain threshold, and then adds 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; reducing the deflection angle between adjacent nodes reduces the turning points of the path, improves the smoothness of the mobile robot's movement, and the turning angle of the mobile robot on the optimized path is small, which facilitates the movement of the mobile robot.

[0019] Since 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 the mobile robot moves toward the optimized path, and avoid the mobile robot exceeding the optimized path when moving toward the optimized path; confirm the forward point forward through the position of the projection point, extend the distance the mobile robot enters the optimized path, reduce the turning radius of the mobile robot directly entering the optimized path, and improve the smoothness of the mobile robot's movement.

[0020] Furthermore, S10 is specifically: Set the speed range of the mobile robot at time t, Vt∈[Vmin,Vmax], Wt∈[Wmin,Wmax]; The moving path between the mobile robot and the forward point is calculated through the formula G(v,ω)=α*dis_angle(v,ω)+β*dis_local(v,ω)+γ*vel(v,ω)+c*dis_goal(v,ω).

[0021] α, β, γ and c are weight coefficients; dis_angle(v, ω) is the cost value of the angle difference between the coordinates of the mobile robot at time t in the moving path to the forward-looking point angle and the body orientation angle.

[0022] dis_local(v, ω) is the cost value of the angle difference between the current coordinates of the mobile robot and the forward-looking point angle and the vehicle body orientation angle; vel(v, ω) is the cost value of the current velocity of the mobile robot; dis_goal(v, ω) is the penalty cost value of the mobile robot exceeding the target node.

[0023] The above method is affected by speed when planning the moving path. If the angle difference between the current position coordinates of the mobile robot and the forward-looking point angle and the body orientation angle is large, the cost value will increase as the speed increases; then the speed is constrained according to the angle difference between the current position coordinates of the mobile robot and the forward-looking point angle and the body orientation angle, and the angle difference between the next position coordinates of the mobile robot at time t and the forward-looking point angle and the body orientation angle 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 optimized 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 optimized path decreases, and the current speed can be maintained. At the same time, the penalty cost value of exceeding the target node is added to the formula to prevent the mobile robot from exceeding the target node.

[0024] Furthermore, S10 also includes the following steps: S10.1. Calculate the coordinates of the mobile robot at time t (x t ,yt ) and the look-ahead point (x p ,y p ) between the third angle θ pt , by moving the robot's body toward angle θ at time t t The third angle θ pt Calculate dis_angle(v, ω).

[0025] S10.2, the calculation method of dis_local(v, ω) is as follows: Calculate the current coordinates (x0, y0) of the mobile robot and the forward point (x p ,y p ) between the fourth angle θ p0 , calculate the current body orientation angle θ0 of the mobile robot and the fourth angle θ p0 The angle difference is obtained by the vehicle body orientation angle θ0 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.

[0026] S10.3. When the mobile robot is at the end node of the moving path, determine whether the node with the shortest distance to the mobile robot on the optimization 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 end node of the moving path and the target node; θ L It is the angle between the line connecting the end node of the moving path and the target node relative to the straight line direction of the preferred path.

[0027] In the above method, when the projection point is inconsistent with the target node, it means that the target node is in front of the projection point, and it does not interfere with the movement of the mobile robot to the forward point; when the projection point is consistent with the target node, if the mobile robot moves in front of the projection point, it will cause the mobile robot to exceed 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 accurately return to the target node.

[0028] Furthermore, S2 is specifically as follows: the formula F(n) = G(n) + H(n) is used; the F(n) values ​​of all nodes are calculated; G(n) is the cost from the initial node to a node, and H(n) is the estimated cost from a node to the target node; F(n) is the evaluation function; the original path is generated through the node with the smallest F(n) value.

[0029] The above method selects the node with the smallest cost between the initial node and the target node through the A* algorithm; generates an original path through the initial node, the target node and the node with the smallest 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.

[0030] Furthermore, through the formula Calculate the value of H(n); (x g ,y g ) is the coordinate of the next node, (x n ,y n ) is the coordinate of a node.

[0031] The above method obtains the estimated cost value through the distance formula between two points, and has good accuracy.

[0032] Furthermore, S6 is specifically: by formula xk=(x n + x m ) / 2; yk=(y n + y m ) / 2, calculate the coordinates of the midpoint (xk, yk), (x n ,y n ) is the coordinate of a node, (x m ,y m ) are the coordinates of another node.

[0033] The above settings make it easy to calculate the midpoint coordinates. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a schematic diagram of the original path in the present invention.

[0035] Figure 2 It is a schematic diagram of the optimization path in the present invention.

[0036] Figure 3 Schematic diagram of the projection point, forward-looking point and mobile robot in the optimized path of the present invention.

[0037] Figure 4 It is a schematic diagram of the mobile robot exceeding the target node in the present invention.

[0038] Figure 5 It is a flow chart of the present invention. DETAILED DESCRIPTION

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

[0040] like Figure 1-5 As shown, a local path planning method for a mobile robot based on 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.

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

[0042] S2. Generate an original path between the initial node and the target node, where the original path is the shortest path between the initial node and the target node.

[0043] S3, preset angle difference threshold; select any three adjacent nodes, calculate a first angle between one node and another node, calculate a second angle between another node and yet another node, and calculate the angle difference between the first angle and the second angle.

[0044] S4, determine whether the current angle difference is greater than the angle difference threshold, if so, proceed to S5, if not, walk along the original path.

[0045] S5. Delete the middle nodes between three adjacent nodes in the original path.

[0046] S6. Calculate the midpoint between the remaining two nodes; in this embodiment, the formula xk=(x n + x m ) / 2; yk=(y n + y m ) / 2, calculate the coordinates of the midpoint (xk, yk), (x n ,y n ) is the coordinate of a node, (x m ,y m ) is the coordinate of another node.

[0047] S7. Update the midpoint as a new node in the original path to form an optimized path.

[0048] S8. Confirm the projection point 1 of the mobile robot on the optimized path. The projection point 1 is the node on the optimized path that is closest to the center of the mobile robot.

[0049] S9, along the forward direction of the optimized path, set the next node in front of projection point 1 as forward point 2, and the coordinates of forward point 2 are (x p ,y p ).

[0050] S10, generating a moving path between the mobile robot and the forward point 2 according to the current speed of the mobile robot and the deviation angle between the moving direction of the current moving trajectory of the mobile robot at the current speed and the forward point 2.

[0051] The above method deletes the nodes with large angle difference between the two nodes, and then adds 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; reducing the deflection angle between adjacent nodes reduces the turning points of the path, improves the smoothness of the mobile robot's movement, and the turning angle of the mobile robot on the optimized path is small, which facilitates the movement of the mobile robot.

[0052] Since 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 the mobile robot moves toward the optimized path, and avoid the mobile robot exceeding the optimized path when moving toward the optimized path; confirm the forward point 2 forward through the position of the projection point 1, extend the distance the mobile robot enters the optimized path, reduce the turning radius of the mobile robot directly entering the optimized path, and improve the smoothness of the mobile robot's movement.

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

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

[0055] 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; and T is the simulation period.

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

[0057] α, β, γ and c are user-defined weight coefficients; dis_angle(v, ω) is the cost value of the angle difference between the coordinates of the mobile robot at time t in the moving path to the forward-looking point 2 and the body orientation angle.

[0058] dis_local(v, ω) is the cost value of the angle difference between the current coordinates of the mobile robot and the forward-looking point 2 and the vehicle body orientation angle; vel(v, ω) is the cost value of the current velocity of the mobile robot; dis_goal(v, ω) is the penalty cost value of the mobile robot exceeding the target node.

[0059] The above method is affected by speed when planning the moving path. If the angle difference between the current position coordinates of the mobile robot and the forward-looking point 2 and the body orientation angle is large, the cost value will increase as the speed increases. Then, the speed is constrained according to the angle difference between the current position coordinates of the mobile robot and the forward-looking point 2 and the body orientation angle, and the angle difference between the next position coordinates of the mobile robot at time t and the forward-looking point 2 and the body orientation angle 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 optimized 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 optimized path decreases, and the current speed can be maintained. At the same time, the penalty cost value of exceeding the target node is added to the formula to prevent the mobile robot from exceeding the target node.

[0060] The calculation method of dis_angle(v, ω) includes: S10.1, calculating the coordinates of the mobile robot at time t (x t ,y t ) and the forward point 2 coordinates (x p ,y p ) between the third angle θ pt , the third angle θ pt The calculation of can be obtained by the coordinates of the mobile robot obtained by the positioning system and the coordinates of the forward point 2 through the angle calculation formula, refer to Figure 3 As shown in Figure 4, the mobile robot at time t is shown in Figure 5. t The third angle θ pt Calculate dis_angle(v, ω). The calculation formula is dis_angle(v, ω)=|θ pt -θ t |=| arctan[(y p -y t ) / (x p -x t )]- θ t |.

[0061] S10.2, calculate the current coordinates (x0, y0) of the mobile robot and the coordinates of the forward point 2 (x p ,y p ) between the fourth angle θ p0 , refer to Figure 3As shown in FIG. 3, the current mobile robot is calculated to determine the current body orientation angle θ0 of the mobile robot and the fourth angle θ p0 The angle difference between θ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.

[0062] S10.3. When the mobile robot is at the end node of the moving path, determine whether the node with the shortest distance to the mobile robot on the optimization 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 end node of the moving path and the target node; θ L It is the angle between the line connecting the end node of the moving path and the target node relative to the straight line direction of the preferred path.

[0063] When projection point 1 is inconsistent with the target node, it means that the target node is in front of projection point 1, and it does not interfere with the movement of the mobile robot to forward point 2; when projection point 1 is consistent with the target node, if the mobile robot moves in front of projection point 1, it will cause the mobile robot to exceed the target node, thus 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.

[0064] Specifically in S2: the formula F(n) = G(n) + H(n) is used to calculate the F(n) values ​​of all nodes; G(n) is the cost from the initial node to a node, H(n) is the estimated cost from a node to the target node; F(n) is the evaluation function; the original path is generated through the node with the smallest F(n) value. Calculate the value of H(n); (x g ,y g ) is the coordinate of the next node, (x n ,y n ) is the coordinate of a node. The estimated cost value is obtained by the distance formula between two points, with good accuracy. The node with the smallest cost value 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 smallest cost 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 local path planning method for a mobile robot based on 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 an 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 a first angle between one node and another node, calculate a second angle between another node and another node, and calculate the angle difference between the first angle and the second angle; S4, determine whether the current angle difference is greater than the angle difference threshold, if so, proceed to S5; S5. Delete the middle nodes between three adjacent nodes in the original path; S6. Calculate the midpoint between the remaining two nodes; S7, updating the midpoint as a new node into the original path to form an optimized path; S8. confirming the projection point of the mobile robot on the optimized path, where the projection point is the node on the optimized path that is closest to the center of the mobile robot; S9, along the forward direction of the optimization path, set the next node in front of the projection point as the forward point (x p ,y p ); S10, generating a moving path between the mobile robot and the forward-looking point according to the current speed of the mobile robot and the deviation angle between the moving direction of the current moving trajectory of the mobile robot at the current speed and the forward-looking point.

2. The DWA-based local path planning method for a mobile robot according to claim 1, characterized in that: S10 includes: Set the speed range of the mobile robot at time t, Vt∈[Vmin,Vmax], Wt∈[Wmin,Wmax]; The moving path between the mobile robot and the forward point is calculated through the formula G(v,ω)=α*dis_angle(v,ω)+β*dis_local(v,ω)+γ*vel(v,ω)+c*dis_goal(v,ω). α, β, γ and c are weight coefficients; dis_angle(v, ω) is the cost value of the angle difference between the coordinates of the mobile robot at time t in the moving path to the forward-looking point and the body orientation angle; dis_local(v, ω) is the cost value of the angle difference between the current coordinates of the mobile robot and the forward-looking point angle and the vehicle body orientation angle; vel(v, ω) is the cost value of the current velocity of the mobile robot; dis_goal(v, ω) is the penalty cost value of the mobile robot exceeding the target node.

3. The DWA-based local path planning method for a mobile robot according to claim 2, characterized in that: S10 also includes the following steps: S10.

1. Calculate the coordinates of the mobile robot at time t (x t ,y t ) and the look-ahead point (x p ,y p ) between the third angle θ pt , by moving the robot's body toward angle θ at time t t The third angle θ pt Calculate dis_angle(v, ω); S10.2, the calculation method of dis_local(v, ω) is as follows: Calculate the current coordinates (x0, y0) of the mobile robot and the forward point (x p ,y p ) between the fourth angle θ p0 , calculate the current body orientation angle θ0 of the mobile robot and the fourth angle θ p0 The angle difference between θ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; S10.

3. When the mobile robot is at the end node of the moving path, determine whether the node with the shortest distance to the mobile robot on the optimization 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 end node of the moving path and the target node; θ L It is the angle between the line connecting the end node of the moving path and the target node relative to the straight line direction of the preferred path.

4. The DWA-based local path planning method for a mobile robot according to claim 1, characterized in that: Specifically in S2: the formula F(n) = G(n) + H(n) is used to calculate the F(n) values ​​of all nodes; G(n) is the cost from the initial node to a node, and H(n) is the estimated cost from a node to the target node; F(n) is the evaluation function; the original path is generated through the node with the smallest F(n) value.

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

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

Images