Robot deceleration obstacle avoidance method based on DWA

Abstract

The invention provides a robot deceleration obstacle avoidance method based on DWA, and the method comprises the steps: reducing the moving speed of a mobile robot when a current angle difference is greater than an angle difference threshold value; when the current angle difference is smaller than the angle difference threshold value, the mobile robot moves at the current speed; the moving speed of the mobile robot to the target node is maximized; the node closest to the obstacle is set as a second deceleration node, and then the mobile robot decelerates after moving to the second deceleration node; the obstacle is prevented from moving and interfering with the mobile robot; by setting the minimum distance between the mobile robot and the obstacle, when the mobile robot approaches the obstacle, the mobile robot stops moving, so that a certain distance is kept between the mobile robot and the obstacle; and the mobile robot is prevented from colliding with obstacles.

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, a patent document with application number 202210544455.4 in China and publication date 2022.8.5 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 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 turning points encountered without considering the size of the turning 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 along the A* algorithm. When encountering a dynamic obstacle, the local DWA algorithm is turned on to reach the destination. This method does not decelerate when encountering a dynamic obstacle. If the method simply decelerates 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 to move; the speed of the mobile robot moving to a 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; 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 comprises the following steps: 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 an initial node and a 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.

[0007] S2. Preset an 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.

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

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

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

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

[0012] S7, the mobile robot moves toward the target node. If an obstacle is found to block the movement of the mobile robot, S8 is performed.

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

[0014] S9, presetting the 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.

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

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

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

[0018] When the mobile robot moves to the target node, it scans in real time whether there are obstacles blocking the mobile robot, sets the node closest to the obstacle as the second deceleration node, and then decelerates after the mobile robot moves to the second deceleration point to avoid the movement of the obstacle and interference with the mobile robot. By setting the minimum distance between the mobile robot and the obstacle, the mobile robot stops moving when it approaches the obstacle, so that the mobile robot and the obstacle are kept at a certain distance, avoiding collision between the mobile robot and the obstacle.

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

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

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

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

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

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

[0025] Figure 1 It is a flow chart of the present invention. DETAILED DESCRIPTION

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

[0027] like Figure 1 As shown, a robot deceleration and obstacle avoidance method based on DWA includes the following steps: 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 an initial node and a 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.

[0028] S2. Preset an 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.

[0029] 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); the coordinates of one node are (Xc, Yc); the angle θ1 of the first angle is determined by the formula arctan*[(Yb-Ya) / (Xb-Xa)], the angle θ2 of the second angle is determined by the formula arctan*[(Yc-Yb) / (Xc-Xb)], and the angle difference between the first angle and the second angle is calculated by |θ2-θ1|. The above calculation method is simple.

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

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

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

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

[0034] S7, the mobile robot moves toward the target node, and if an obstacle is found to block the movement of the mobile robot, S8 is performed. In this embodiment, the unknown movable obstacle may be a person or other mobile robot, and in this embodiment, the obstacle is a person.

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

[0036] In this embodiment, 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. The method of determining all nodes by an interpolation algorithm is simple. Specifically, the N coordinate points between the mobile robot and the obstacle are calculated by an interpolation algorithm, and the node closest to the obstacle that coincides with one of the coordinate points is determined, and the node closest to the obstacle is set as the second deceleration node.

[0037] S9, presetting the 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.

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

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

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

[0041] When the mobile robot moves to the target node, it scans in real time whether there are obstacles blocking the mobile robot, sets the node closest to the obstacle as the second deceleration node, and then decelerates after the mobile robot moves to the second deceleration point to avoid the movement of the obstacle and interference with the mobile robot. By setting the minimum distance between the mobile robot and the obstacle, the mobile robot stops moving when it approaches the obstacle, so that the mobile robot and the obstacle are kept at a certain distance, avoiding collision between the mobile robot and the obstacle.

[0042] In the above method, the formula Calculate the maximum planned speed of the mobile robot moving to the first deceleration point or the second deceleration point; when calculating the speed Vmax of the mobile robot moving to the first deceleration point, as is a*s, s is the distance between the mobile robot and the first deceleration point, and a is the maximum deceleration of the car; 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 car. According to the current distance between the mobile robot and the first deceleration point, adjust the moving speed of the mobile robot; according to the current distance between the mobile robot and the second deceleration point, adjust the moving speed of the mobile robot.

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

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

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

[0046] The method comprises the following steps: when a first deceleration point is arranged in front of the mobile robot, the mobile robot moves toward the first deceleration point at a maximum planned speed to perform 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.

[0047] When a second deceleration point is set 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 can stop stably and safely in front of the obstacle.

[0048] The working principle of the present invention is as follows: a preset angle difference threshold is set, 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.

[0049] When the mobile robot moves to the target node, it scans in real time whether there are obstacles blocking the mobile robot, sets the node closest to the obstacle as the second deceleration node, and then decelerates after the mobile robot moves to the second deceleration point to avoid the movement of the obstacle and interference with the mobile robot. By setting the minimum distance between the mobile robot and the obstacle, the mobile robot stops moving when it approaches the obstacle, so that the mobile robot and the obstacle are kept at a certain distance, avoiding collision between the mobile robot and the obstacle.

Claims

1. A robot deceleration and obstacle avoidance method based on DWA, characterized by: The following steps are involved: S1. Preset a map and construct a global coordinate system in the map. 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, the target node is the end point of the mobile robot, and 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, 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; S3, determine whether the current angle difference is greater than the angle difference threshold, if so, proceed to S4; S4, setting the middle node of three adjacent nodes in the planned path as an inflection point; S5, along the forward direction of the planned path, setting the previous node of the turning 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 speed and passes the turning point; S7, the mobile robot moves toward the target node. If an obstacle is found to block 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 the 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 one node are (Xa, Ya); the coordinates of another node are (Xb, Yb); the coordinates of one node are (Xc, Yc); the angle θ1 of the first angle is determined by the formula arctan*[(Yb-Ya) / (Xb-Xa)], the angle θ2 of the second angle is determined by the formula arctan*[(Yc-Yb) / (Xc-Xb)], and 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