Robot dynamic obstacle avoidance method based on fusion of A* and artificial potential field algorithm

Abstract

The invention discloses a robot dynamic obstacle avoidance method based on A * and artificial potential field algorithm fusion, and solves the problems of path redundancy and poor real-time performance in a robot dynamic obstacle avoidance process. The method comprises the steps that S1, environment information is initialized, and an environment two-dimensional grid map is established and comprises the current position of a robot, the target position, the obstacle position and other information; s2, an environment map is known, an A * algorithm cost evaluation function is optimized for global path planning, and a global optimal path is obtained. And S3, optimizing a global path through a Bezier curve method. And S4, according to the position of the robot, constructing a target point gravitational potential field, an obstacle repulsive force potential field and a global path gravitational potential field in real time. And S5, according to the potential field, the gravitation and repulsive force borne by the robot at the current position are calculated, the magnitude and direction of the resultant force borne by the robot at the current moment can be obtained through integration, and the position of the robot at the next moment is determined. And S6, judging whether the robot reaches the target position or not according to the position at the next moment, if so, ending the task, otherwise, returning to S4 until the robot reaches the target position.

Description

Technical field:

[0001] The invention relates to the technical field of mobile robot path planning and dynamic obstacle avoidance, and in particular to a robot dynamic obstacle avoidance method. Technical background:

[0002] With the rapid development of science and technology, mobile robots have been widely used in various industries such as smart logistics, military, firefighting, and agricultural production. The application scenarios are becoming more and more complex, and the requirements for robot path planning and dynamic obstacle avoidance technology are also increasing. At present, domestic and foreign researchers divide path planning into global path planning and local path planning based on whether the environmental information is known. The global path refers to planning a safe, collision-free optimal path for the robot in a static scene where the environmental information is completely known. Local path planning refers to dynamically generating a path based on local environmental changes in a dynamic environment where the environmental information is not completely known.

[0003] Global path planning has high requirements for environmental knowledge. It is usually optimal and meets the principle of shortest path, but its real-time performance is poor and it cannot meet the obstacle avoidance problem in dynamic environment. Local path planning has high real-time performance and meets the requirements of dynamic obstacle avoidance, but local path planning algorithms usually do not meet the optimality and have problems such as path node redundancy. Based on this, a robot path planning method is needed that can meet both global optimality and real-time dynamic obstacle avoidance. Summary of the invention:

[0004] In order to solve the technical problems of poor real-time performance and path redundancy faced by existing path planning, this application provides a robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithm to meet the global optimality of path planning and real-time dynamic obstacle avoidance requirements, and improve and optimize the robot navigation problem in a dynamic environment. The present invention proposes a robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithm, the method comprising:

[0005] S1: Initialize the environment information and build a two-dimensional grid map of the environment, including the robot's current position, target position, obstacle position and other information.

[0006] S2: Given a known environment map, optimize the A* algorithm cost evaluation function to perform global path planning and obtain the global optimal path.

[0007] S3: Optimize the global path by Bezier curve method.

[0008] S4: According to the robot's location, the target point gravitational potential field, obstacle repulsive potential field, and global path gravitational potential field are constructed in real time.

[0009] S5: Calculate the gravitational force and repulsive force on the robot's current position according to the potential field, integrate the magnitude and direction of the resultant force on the robot at the current moment, and determine the position of the robot at the next moment.

[0010] S6: Determine whether the robot has reached the target position based on the position at the next moment. If it has reached the target position, the task is completed. Otherwise, return to S4 until the robot reaches the target position.

[0011] Optionally, the cost evaluation function of the A* algorithm in S2 is: Among them, f(n) represents the cost value of the current node n, G(n) represents the cost value from the starting point to the current node n, and H(n) represents the estimated cost value from the current node n to the target point. (1+d / D) is the distance decay factor introduced, d represents the Euclidean distance from the current node n to the target point, and D represents the Euclidean distance from the starting point to the target point. When G(n) is calculated using the Euclidean distance formula: When H(n) is estimated using Manhattan distance: H(n)=|xx t |+|yy t | The robot's starting position is (x0, y0), the current node position coordinates are (x, y), and the target position coordinates are (x t ,y t ).

[0012] Optionally, the Bezier curve method in S3 is: The global path points P0, P1, P2, ..., P n There are n+1 control points in total, which are connected in sequence to form a control polygon. A Bezier curve is used to approximate this polygon, and then the shape of the curve is changed by adjusting the coordinates of the control points to smooth the global path.

[0013] Optionally, each potential field in S4 is: The robot is simplified into a mass point, and a reasonable virtual potential field is constructed in the environment, where the target point provides a gravitational potential field to the robot, the obstacle provides a repulsive potential field to the target point, and the global path point provides an attractive force to the robot. att (q): Among them, q is the current position of the mobile robot, k is the gravitational gain coefficient, and d(q,q tar ) is the Euclidean distance from the current position to the target point. Obstacle repulsive force field U rep (q): Among them, β is the repulsive force gain coefficient, d(q, q obs ) is the distance from the current position to the obstacle, d0 is the action radius of the obstacle, and d(q, q tar ) is the distance from the current position to the target point. Global path point gravitational potential field: Among them, β tra is the trajectory gravitational gain, d(q, q tar ) is the distance from the current position to the trajectory point, d(q, q tar ) is the distance from the current position to the target point, and d tra0 is the action radius of the trajectory gravity.

[0014] Optionally, the resultant force calculation and the next moment position calculation in S5 are as follows: Take the negative gradient of the potential field function to obtain the corresponding potential field force. Gravitational force of the target point F att : Repulsive force of the obstacle F rep : Global path gravity F tra : F = F att + F rep Resultant force F sum : F sum = F + F tra Next moment position P of the robot (x,y) : Among them, P (x0,y0) is the current position of the mobile robot, step is the movement step size, and UnitVec_F sum is the direction vector of F sum .

[0015] Optionally, the judgment of whether the target position is reached in S6 is as follows: Among them, s is the Euclidean distance from the next moment position to the target position, s0 is the distance threshold. When s < s0, it is considered that the robot reaches the target point; otherwise, it is considered that the robot does not reach the target position.

[0016] The beneficial effects of the present invention are as follows:

[0017] The present invention first plans the global optimal path through the A* algorithm, and then dynamically adjusts the local path through the artificial potential field algorithm locally, thereby reducing the risk of collision between the robot and dynamic obstacles. The present invention introduces a distance attenuation factor into the A* algorithm, so that the A* algorithm expansion node moves quickly toward the target point, and reduces the node redundancy in the search process, thereby improving the algorithm search rate. The present invention improves the repulsive potential field function of the artificial potential field algorithm, introduces the distance between the mobile robot and the target point as an adjustment factor, solves the problem of the artificial potential field falling into the local optimum and the target point being unreachable, and introduces a global path potential field at the same time, so that the local path is as close to the global path as possible, reducing the redundancy of the local path. The fusion algorithm proposed by the present invention meets the shortest path principle while meeting the real-time dynamic obstacle avoidance requirements. Description of the drawings:

[0018] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0019] Figure 1 This is a flow chart of a robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithms.

[0020] Figure 2 Schematic diagram of the results of the A* algorithm and the IA* algorithm.

[0021] Figure 3 Optimize global path diagrams for Bezier curves.

[0022] Figure 4 Schematic diagram of force analysis of the robot under the improved artificial potential field algorithm.

[0023] Figure 5 Schematic diagram of the mobile robot trajectory Specific implementation method:

[0024] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application. Embodiment 1:

[0025] Embodiment 1 of the present application provides a robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithms, which is described in detail below with reference to the accompanying drawings.

[0026] See also Figure 1 , which is a flow chart of a robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithm provided by this application.

[0027] The specific steps include:

[0028] S1: Initialize the environment information and build a two-dimensional grid map of the environment, including the robot's current position, target position, obstacle position and other information.

[0029] S2: Given a known environment map, optimize the A* algorithm cost evaluation function to perform global path planning and obtain the global optimal path.

[0030] S3: Optimize the global path by Bezier curve method.

[0031] S4: According to the robot's location, the target point gravitational potential field, obstacle repulsive potential field, and global path gravitational potential field are constructed in real time.

[0032] S5: Calculate the gravitational force and repulsive force on the robot's current position according to the potential field, integrate the magnitude and direction of the resultant force on the robot at the current moment, and determine the position of the robot at the next moment.

[0033] S6: Determine whether the robot has reached the target position based on the position at the next moment. If it has reached the target position, the task is completed. Otherwise, return to S4 until the robot reaches the target position.

[0034] Furthermore, the cost evaluation function of the A* algorithm in S2 is: Among them, f(n) represents the cost value of the current node n, G(n) represents the cost value from the starting point to the current node n, and H(n) represents the estimated cost value from the current node n to the target point. (1+d / D) is the distance decay factor introduced, d represents the Euclidean distance from the current node n to the target point, and D represents the Euclidean distance from the starting point to the target point. When G(n) is calculated using the Euclidean distance formula: When H(n) is estimated using Manhattan distance: H(n)=|xx t |+|yy t | The robot's starting position is (x0, y0), the current node position coordinates are (x, y), and the target position coordinates are (x t ,y t ).

[0035] refer to Figure 2 Schematic diagram of the results of the traditional A* algorithm and the improved IA* algorithm.

[0036] Furthermore, the Bezier curve method in S3 is: The global path points P0, P1, P2, ..., P n There are n+1 control points in total, which are connected in sequence to form a control polygon. A Bezier curve is used to approximate this polygon, and then the shape of the curve is changed by adjusting the coordinates of the control points to smooth the global path.

[0037] refer to Figure 3 This is a schematic diagram of the Bezier curve optimization results.

[0038] Furthermore, the potential fields in S4 are: The robot is simplified into a mass point, and a reasonable virtual potential field is constructed in the environment, where the target point provides a gravitational potential field to the robot, the obstacle provides a repulsive potential field to the target point, and the global path point provides an attractive force to the robot. att (q): Among them, q is the current position of the mobile robot, k is the gravitational gain coefficient, and d(q,q tar ) is the Euclidean distance from the current position to the target point. Obstacle repulsive force field U rep (q): Among them, β is the repulsion gain coefficient, d(q,q obs ) is the distance from the current position to the obstacle, d0 is the radius of the obstacle, d(q,q tar ) is the distance from the current position to the target point. Global waypoint gravitational potential field: Among them, β tra is the trajectory gravitational gain, d(q,q tar ) is the distance from the current position to the trajectory point, d(q,q tar ) is the distance from the current position to the target point, d tra0 is the radius of action of trajectory gravity.

[0039] Furthermore, the resultant force calculation and the position calculation at the next moment in S5 are: The negative gradient of the potential field function is used to obtain the corresponding potential field force. att : Obstacle repulsion force F rep : F = F att + F rep Global path attraction force F tra : Resultant force F sum : F sum = F + F tra Next moment position P of the robot (x,y) : wherein, P (x0,y0) is the current position of the mobile robot, step is the moving step size, and UnitVec_F sum is the direction vector of F sum .

[0040] s reference Figure 4 , which is a schematic diagram of the force analysis of the robot under the improved artificial potential field algorithm.

[0041] Furthermore, the judgment of whether the target position is reached in S6 is as follows: wherein, s is the Euclidean distance from the next moment position to the target position, s0 is the distance threshold. When s < s0, it is considered that the robot reaches the target point; otherwise, it is considered that the robot does not reach the target position.

[0042] Reference Figure 5 , which is a schematic diagram of the robot trajectory of the fusion algorithm.

Claims

1. A robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithm, characterized in that: It includes the following steps: S1: Initialize the environmental information and establish a two-dimensional environmental grid map, including information such as the current position of the robot, the target position, and the obstacle positions. S2: Given the environmental map, optimize the cost evaluation function of the A* algorithm for global path planning to obtain the globally optimal path. S3: Optimize the global path by the Bezier curve method. S4: According to the position of the robot, construct in real time the gravitational potential field of the target point, the repulsive potential field of the obstacle, and the gravitational potential field of the global path. S5: Calculate the gravitational force and repulsive force acting on the current position of the robot according to the potential fields respectively. The resultant force magnitude and direction acting on the robot at the current moment can be integrated to determine the position of the robot at the next moment. S6: Determine whether the robot has reached the target position according to the position at the next moment. If it has reached, the task ends; otherwise, return to S4 until the robot reaches the target position.

2. The robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithm according to claim 1 is characterized in that: The cost evaluation function of the A* algorithm in S2 is: Among them, f(n) represents the cost value of the current node n, G(n) represents the cost value from the starting point to the current node n, and H(n) represents the cost estimated value from the current node n to the target point. (1 + d / D) is the introduced distance attenuation factor, d represents the Euclidean distance from the current node n to the target point, and D represents the Euclidean distance from the starting point to the target point. When G(n) is calculated using the Euclidean distance formula: When H(n) is estimated using the Manhattan distance: H(n)=|x-x t |+|y-y t | The robot's starting position is (x0, y0), the current node position coordinates are (x, y), and the target position coordinates are (x t ,y t ).

3. The robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithm according to claim 1 is characterized in that: The Bezier curve method in S3 is: The global path points P0, P1, P2, ..., P n There are n+1 control points in total, which are connected in sequence to form a control polygon. A Bezier curve is used to approximate this polygon, and then the shape of the curve is changed by adjusting the coordinates of the control points to smooth the global path.

4. The robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithm according to claim 1 is characterized in that: The potential fields in S4 are: The robot is simplified into a mass point, and a reasonable virtual potential field is constructed in the environment, where the target point provides a gravitational potential field to the robot, the obstacle provides a repulsive potential field to the target point, and the global path point provides an attractive force to the robot. att (q): Among them, q is the current position of the mobile robot, k is the gravitational gain coefficient, and d(q,q tar ) is the Euclidean distance from the current position to the target point. Obstacle repulsive force field U rep (q): Among them, β is the repulsion gain coefficient, d(q,q obs ) is the distance from the current position to the obstacle, d0 is the radius of the obstacle, d(q,q tar ) is the distance from the current position to the target point. Gravitational potential field of global path points: Among them, β tra is the trajectory gravitational gain, d(q,q tar ) is the distance from the current position to the trajectory point, d(q,q tar ) is the distance from the current position to the target point, d tra0 is the radius of action of trajectory gravity.

5. The robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithm according to claim 1 is characterized in that: The resultant force calculation and the next moment position calculation in S5 are: Take the negative gradient of the potential field function to obtain the corresponding potential field force. Target point gravity F att : Obstacle repulsion F rep : F=F att +F rep Global path gravity F tra : Combined Force F sum : F sum =F+F tra The robot's next position P (x,y) : Among them, P (x0,y0) is the current position of the mobile robot, step is the moving step length, UnitVec_F sum F sum The direction vector of .

6. The robot dynamic obstacle avoidance method based on the fusion of A* and artificial potential field algorithm according to claim 1 is characterized in that: The judgment of whether the target position is reached in S6 is: Among them, s is the Euclidean distance from the position at the next moment to the target position, and s0 is the distance threshold. When s < s0, it is considered that the robot has reached the target point; otherwise, it is considered that the robot has not reached the target position.

Images