A robot deceleration and obstacle avoidance method based on DWA

Abstract

The present invention provides a robot deceleration and obstacle avoidance method based on DWA. When the current angle difference is greater than an angle difference threshold, the moving speed of the mobile robot is reduced; when the current angle difference is less than the angle difference threshold, the mobile robot maintains the current speed; the speed of the mobile robot moving to the target node is maximized; the node closest to the obstacle is set as the second deceleration node, and then the mobile robot moves to the second deceleration point and decelerates; the movement of the obstacle is prevented from interfering with the mobile robot; by setting a minimum distance between the mobile robot and the obstacle, the mobile robot stops moving when the mobile robot approaches the obstacle, so that a certain distance is maintained between the mobile robot and the obstacle, and the mobile robot is prevented from colliding with the obstacle.

Description

Technical Field

[0001] The present invention relates to the technical field of AGV path planning, and in particular to a robot deceleration and obstacle avoidance method based on DWA. Background Art

[0002] With the development of industrial technology, robots have emerged. Among them, mobile robots have become the focus of researchers due to their strong mobility. Mobile robots need to consider path planning and obstacle avoidance during movement, and the research focus of mobile robots includes path planning and obstacle avoidance methods.

[0003] For example, the patent document with application number 202210544455.4 in China and publication date of August 5, 2022, discloses an AGV path planning method based on an improved DWA algorithm in a dynamic environment. It discloses the optimization of the DWA evaluation function when the A* algorithm and the DWA algorithm are integrated for path planning. The AGV starts from the starting point and executes the DWA planning algorithm towards the end point to reach the end point. There is a deviation of an angle θ between the AGV speed direction and the direction of movement to the next path point. The AGV decelerates before reaching the end point. After reaching the end point, the speed is 0 and the angle begins to be adjusted.

[0004] This method decelerates at all inflection points encountered, without considering the size of the inflection point angle, and cannot maximize the speed of movement. At the same time, when the robot does not detect a conflicting obstacle, it always moves according to the A* algorithm. When encountering a dynamic obstacle, it activates the local DWA algorithm to reach the destination. This method does not decelerate when encountering a dynamic obstacle. If a simple deceleration method is used during the obstacle avoidance process, if the deceleration is too large for smaller obstacles, it will not be able to ensure reliable movement, and will interfere with the dynamic obstacle. Summary of the Invention

[0005] The present invention provides a robot deceleration and obstacle avoidance method based on DWA. When the current angle difference is greater than an angle difference threshold, the moving speed of the mobile robot is reduced; when the current angle difference is less than the angle difference threshold, the mobile robot maintains the current speed; the speed of the mobile robot moving toward the target node is maximized; the node closest to the obstacle is set as the second deceleration node, and the mobile robot decelerates after moving to the second deceleration point; and the movement of the obstacle is prevented from interfering with the mobile robot.

[0006] To achieve the above object, the technical solution of the present invention is: a robot deceleration and obstacle avoidance method based on DWA includes the following steps:

[0007] S1. Preset a map and build a global coordinate system in the map. Each coordinate point in the global coordinate system is a node; preset the initial node and the target node. The initial node is the starting point of the mobile robot, and the target node is the end point of the mobile robot. Generate a planned path between the initial node and the target node.

[0008] S2. Preset angle difference threshold; select any three adjacent nodes in the planned path, where the three adjacent nodes are the first node, the second node, and the third node, calculate the first angle between the line connecting the first node and the second node and the positive direction of the X-axis, calculate the second angle between the line connecting the second node and the third node and the node in the positive direction of the X-axis, and calculate the angle difference between the first angle and the second angle.

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

[0010] S4. Set the middle node between three adjacent nodes in the planned path as an inflection point.

[0011] S5. Along the forward direction of the planned path, set the previous node of the turning point in the planned path as the first deceleration point.

[0012] S6. The mobile robot moves along the planned path and decelerates at the first deceleration point; the mobile robot reduces its speed and passes through the turning point.

[0013] S7, the mobile robot moves toward the target node. If an obstacle is found that blocks the movement of the mobile robot, proceed to S8.

[0014] S8. Obtain the position of the obstacle; set the node between the mobile robot and the obstacle that is closest to the obstacle as the second deceleration node.

[0015] S9. Preset a minimum distance between the mobile robot and the obstacle; the mobile robot moves toward the obstacle along the planned path and decelerates at the second deceleration point.

[0016] S10: When the current distance between the mobile robot and the obstacle is reduced to the minimum distance, the mobile robot stops moving; after the obstacle disappears, proceed to S11.

[0017] S11. The mobile robot continues to move towards the target node.

[0018] The above method presets an angle difference threshold, and adjusts the speed of the mobile robot by judging the size of the current angle difference and the angle difference threshold; when the current angle difference is greater than the angle difference threshold, the moving speed of the mobile robot is reduced; when the current angle difference is less than the angle difference threshold, the mobile robot maintains the current speed; the speed of the mobile robot moving to the target node is maximized; by setting a first deceleration point before the turning point, the mobile robot decelerates after moving to the first deceleration point; so that the mobile robot will not deviate from the planned path when passing the turning point.

[0019] As the mobile robot moves toward its target node, it scans for obstacles in real time. The node closest to the obstacle is set as the second deceleration node, and the robot then decelerates after moving to the second deceleration point, preventing the obstacle from moving and interfering with the robot. By setting a minimum distance between the mobile robot and the obstacle, the robot stops moving when it approaches the obstacle, maintaining a certain distance between them and preventing collisions.

[0020] Furthermore, in S8, all nodes between the mobile robot and the obstacle are determined by an interpolation algorithm, and the node closest to the obstacle is set as the second deceleration node.

[0021] The above method determines all nodes through interpolation algorithm, and the method is simple.

[0022] Furthermore, in S2, the coordinates of the first node are (Xa, Ya); the coordinates of the second node are (Xb, Yb); and the coordinates of the third node are (Xc, Yc). The first angle θ1 is determined by the formula arctan*[(Yb - Ya) / (Xb - Xa)], and the second angle θ2 is determined by the formula arctan*[(Yc - Yb) / (Xc - Xb)]. The angular difference between the first and second angles is calculated by |θ2 - θ1|. The above calculation method is simple.

[0023] Furthermore, through the formula Calculate the maximum planned speed of the mobile robot moving to the first deceleration point or the second deceleration point; set the minimum driving speed of the mobile robot, s is the distance between the mobile robot and the first deceleration point, and a is the maximum deceleration of the car.

[0024] Furthermore, in S6 and S11, when the mobile robot moves, the speed difference between the maximum planned speed and the driving speed is determined; if the driving speed is greater than the maximum planned speed, the mobile robot moves at the maximum planned speed.

[0025] The above method decelerates the mobile robot in advance to ensure the deceleration effect of the mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 Flowchart of the present invention. DETAILED DESCRIPTION

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

[0028] like Figure 1 As shown, a robot deceleration and obstacle avoidance method based on DWA includes the following steps:

[0029] S1. Preset a map and build a global coordinate system in the map. Each coordinate point in the global coordinate system is a node; preset the initial node and the target node. The initial node is the starting point of the mobile robot, and the target node is the end point of the mobile robot. Generate a planned path between the initial node and the target node.

[0030] S2. Preset angle difference threshold; select any three adjacent nodes in the planned path, 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.

[0031] In this embodiment, the angle difference threshold is 10°. The coordinates of one node are (Xa, Ya); the coordinates of another node are (Xb, Yb); and the coordinates of one node are (Xc, Yc). The first angle θ1 is determined by the formula arctan*[(Yb - Ya) / (Xb - Xa)], and the second angle θ2 is determined by the formula arctan*[(Yc - Yb) / (Xc - Xb)]. The angular difference between the first and second angles is calculated using |θ2 - θ1|. This calculation method is simple.

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

[0033] S4. Set the middle node between three adjacent nodes in the planned path as an inflection point.

[0034] S5. Along the forward direction of the planned path, set the previous node of the turning point in the planned path as the first deceleration point.

[0035] S6. The mobile robot moves along the planned path and decelerates at the first deceleration point; the mobile robot reduces its speed and passes through the turning point.

[0036] S7, the mobile robot moves toward the target node. If an obstacle is found that blocks the movement of the mobile robot, S8 is performed. In this embodiment, the unknown movable obstacle may be a person or other mobile robot. In this embodiment, the obstacle is a person.

[0037] S8. Obtain the position of the obstacle; set the node between the mobile robot and the obstacle that is closest to the obstacle as the second deceleration node.

[0038] In this embodiment, an interpolation algorithm is used to determine all nodes between the mobile robot and the obstacle, and the node closest to the obstacle is set as the second deceleration node. Determining all nodes using an interpolation algorithm is simple. Specifically, the interpolation algorithm calculates N coordinate points between the mobile robot and the obstacle, and the node closest to the obstacle that coincides with one of the coordinate points is determined. This node is then set as the second deceleration node.

[0039] S9. Preset a minimum distance between the mobile robot and the obstacle; the mobile robot moves toward the obstacle along the planned path and decelerates at the second deceleration point.

[0040] S10: When the current distance between the mobile robot and the obstacle is reduced to the minimum distance, the mobile robot stops moving; after the obstacle disappears, proceed to S11.

[0041] S11. The mobile robot continues to move towards the target node.

[0042] The above method presets an angle difference threshold, and adjusts the speed of the mobile robot by judging the size of the current angle difference and the angle difference threshold; when the current angle difference is greater than the angle difference threshold, the moving speed of the mobile robot is reduced; when the current angle difference is less than the angle difference threshold, the mobile robot maintains the current speed; the speed of the mobile robot moving to the target node is maximized; by setting a first deceleration point before the turning point, the mobile robot decelerates after moving to the first deceleration point; so that the mobile robot will not deviate from the planned path when passing the turning point.

[0043] As the mobile robot moves toward its target node, it scans for obstacles in real time. The node closest to the obstacle is set as the second deceleration node, and the robot then decelerates after moving to the second deceleration point, preventing the obstacle from moving and interfering with the robot. By setting a minimum distance between the mobile robot and the obstacle, the robot stops moving when it approaches the obstacle, maintaining a certain distance between them and preventing collisions.

[0044] In the above method, the formula Calculate the maximum planned speed for the mobile robot to move to the first or second deceleration point. When calculating the speed Vmax of the mobile robot moving to the first deceleration point, as is a*s, where s is the distance between the mobile robot and the first deceleration point, and a is the maximum deceleration of the vehicle. When calculating the speed V of the mobile robot moving to the second deceleration point, s is the distance between the mobile robot and the second deceleration point, and a is the maximum deceleration of the vehicle. Adjust the mobile robot's speed based on the current distance between the mobile robot and the first deceleration point; adjust the mobile robot's speed based on the current distance between the mobile robot and the second deceleration point.

[0045] Set the speed at which the mobile robot moves.

[0046] In S6 and S9, when the mobile robot moves, the speed difference between the maximum planned speed and the driving speed is determined; if the driving speed is greater than the maximum planned speed, the mobile robot moves at the maximum planned speed; otherwise, the speed of the mobile robot is not adjusted.

[0047] In this way, the mobile robot is decelerated in advance to ensure the deceleration effect of the mobile robot.

[0048] According to the method, when a first deceleration point is provided in front of the mobile robot, the mobile robot moves toward the first deceleration point at a maximum planned speed and performs a first deceleration; the deceleration effect of the mobile robot at the first deceleration point is good; after reaching the first deceleration point, the mobile robot performs a second deceleration and moves toward the turning point; thus, the safety of the mobile robot when passing the turning point is ensured.

[0049] When a second deceleration point is provided in front of the mobile robot, the mobile robot moves toward the second deceleration point at the maximum planned speed and performs the first deceleration; the deceleration effect of the mobile robot at the second deceleration point is good; after reaching the second deceleration point, the mobile robot performs a second deceleration and moves toward the obstacle; by reducing the speed of the mobile robot twice, the mobile robot is able to stop stably in front of the obstacle safely.

[0050] The working principle of the present invention is as follows: an angle difference threshold is preset, and the speed of the mobile robot is adjusted by judging the size of the current angle difference and the angle difference threshold; when the current angle difference is greater than the angle difference threshold, the moving speed of the mobile robot is reduced; when the current angle difference is less than the angle difference threshold, the mobile robot maintains the current speed to move; the speed of the mobile robot moving to the target node is maximized; by setting a first deceleration point before the turning point, the mobile robot decelerates after moving to the first deceleration point; so that the mobile robot will not deviate from the planned path when passing the turning point.

[0051] As the mobile robot moves toward its target node, it scans for obstacles in real time. The node closest to the obstacle is set as the second deceleration node, and the robot then decelerates after moving to the second deceleration point, preventing the obstacle from moving and interfering with the robot. By setting a minimum distance between the mobile robot and the obstacle, the robot stops moving when it approaches the obstacle, maintaining a certain distance between them and preventing collisions.

Claims

1. A robot deceleration and obstacle avoidance method based on DWA, characterized by: The following steps are involved: S1. Preset the map and build a global coordinate system in the map. 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. A planned path is generated between the initial node and the target node. S2, preset angle difference threshold; select any three adjacent nodes in the planned path, where the three adjacent nodes are the first node, the second node, and the third node, calculate a first angle between the line connecting the first node and the second node and the positive direction of the X-axis, calculate a second angle between the line connecting the second node and the third node and the positive direction of the X-axis, and calculate the angle difference between the first angle and the second angle; S3, determine whether the current angle difference is greater than the angle difference threshold, if so, proceed to S4; S4, setting the middle node between three adjacent nodes in the planned path as an inflection point; S5. Along the forward direction of the planned path, set the previous node of the inflection point in the planned path as the first deceleration point; S6, the mobile robot moves along the planned path and decelerates at the first deceleration point; The mobile robot reduces its speed and passes the turning point; S7, the mobile robot moves towards the target node. If an obstacle is found that blocks the movement of the mobile robot, S8 is performed; S8, obtaining the position of the obstacle; setting the node between the mobile robot and the obstacle that is closest to the obstacle as the second deceleration node; S9, presetting a minimum distance between the mobile robot and the obstacle; the mobile robot approaches the obstacle and decelerates at the second deceleration point; S10: When the current distance between the mobile robot and the obstacle is reduced to the minimum distance, the mobile robot stops moving; after the obstacle disappears, proceed to S11; S11. The mobile robot continues to move towards the target node.

2. The robot deceleration and obstacle avoidance method based on DWA according to claim 1, characterized in that: In S8, all nodes between the mobile robot and the obstacle are determined by an interpolation algorithm, and the node closest to the obstacle is set as the second deceleration node.

3. The robot deceleration and obstacle avoidance method based on DWA according to claim 1, characterized in that: In S2, the coordinates of the first node are (Xa, Ya); the coordinates of the second node are (Xb, Yb); and the coordinates of the third node are (Xc, Yc). The angle θ1 of the first angle is determined by the formula arctan*[(Yb-Ya) / (Xb-Xa)], and the angle θ2 of the second angle is determined by the formula arctan*[(Yc-Yb) / (Xc-Xb)]. The angle difference between the first angle and the second angle is calculated by |θ2-θ1|.

4. The robot deceleration and obstacle avoidance method based on DWA according to claim 1, characterized in that: By formula Calculate the maximum planned speed of the mobile robot moving to the first deceleration point or the second deceleration point; set the minimum driving speed of the mobile robot; s is the distance between the mobile robot and the first deceleration point, and a is the maximum deceleration of the car.

5. The robot deceleration and obstacle avoidance method based on DWA according to claim 4, characterized in that: In S6 and S9, when the mobile robot moves, the speed difference between the maximum planned speed and the driving speed is determined; if the driving speed is greater than the maximum planned speed, the mobile robot moves at the maximum planned speed.

Images