Mobile robot path planning algorithm based on VoronoiObstacle field

Abstract

The invention discloses a mobile robot path planning algorithm based on a VoronoiObstacle field, the VoronoiObstacle field is constructed by using a hierarchical idea, and a path is planned by using a heuristic search algorithm, and the method comprises the following specific steps: S1, constructing a potential field based on an obstacle; s2, constructing a potential field based on the Voronoi diagram; s3, a VoronoiObstacle field based on the barrier potential field and the Voronoi diagram potential field is established; and S4, a heuristic path planning search algorithm based on a VoronoiObstacle field is carried out. The invention discloses a global path planning method based on a VoronoiObstacle field, which is used for autonomous navigation of a mobile robot, and comprises the following steps: firstly, providing information of a scene for path planning, information of a planning starting point and information of a planning end point by an upper-layer system, and then constructing the VoronoiObstacle field by adopting a hierarchical thought, on the premise of ensuring that the algorithm can obtain an optimal or suboptimal safe distance path, the operation time of the algorithm is shortened.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot path planning algorithms, and in particular to a mobile robot path planning algorithm based on a Voronoi_Obstacle field. Background Art

[0002] A mobile robot is an autonomous robotic system that can move, perform tasks or complete specific functions without human intervention. These robots are usually equipped with various sensors, navigation systems and control algorithms, enabling them to perceive the surrounding environment, plan paths and move autonomously. Mobile robots can work in various environments, including indoor and outdoor spaces, and have a wide range of applications such as logistics and warehousing, service robots, agriculture, healthcare, security and inspection, etc. They can have different forms and functions, including wheeled robots, legged robots, drones, etc., to meet the needs of different tasks. In recent years, mobile robots have attracted widespread attention from all walks of life and have been widely used in industry, agriculture, services, search and rescue and other fields. Path planning, as one of the key technologies of intelligent mobile robots, has become a hot topic of research in various fields. Path planning algorithms can be divided into path optimal algorithms, safety distance optimal algorithms, energy optimal algorithms, etc. according to different work requirements. Among them, path optimal algorithms mainly include A* algorithm, D* algorithm, Dijkstra algorithm, RRT algorithm, genetic algorithm, particle swarm algorithm, Q-learning algorithm, etc.; safety distance optimal algorithms mainly include artificial potential field method, Voronoi diagram algorithm, etc.; energy optimal algorithms mainly include: energy optimal algorithm based on random dynamic orthogonal level set, energy consumption optimal path planning based on improved A* algorithm, etc.

[0003] However, the existing Voronoi diagram algorithm has the following problems: high computational complexity and long computational time; when the space becomes larger, the burrs increase, the grid becomes denser, etc., the traditional path planning calculation based on the Voronoi diagram is slow. To solve the above problems, the present invention proposes a mobile robot path planning algorithm based on the Voronoi_Obstacle field, which adopts a hierarchical idea to construct the Voronoi_Obstacle field and uses a heuristic search algorithm to plan the path. Summary of the invention

[0004] The purpose of the present invention is to provide a mobile robot path planning algorithm based on the Voronoi_Obstacle field, which solves the technical problem that the traditional Voronoi field has high calculation complexity and long calculation time; when the space becomes larger, the burrs increase, the grid becomes denser, etc., the traditional path planning calculation based on the Voronoi diagram is slow.

[0005] To achieve the above object, the present invention provides the following technical solution: a mobile robot path planning algorithm based on Voronoi_Obstacle field, which adopts a hierarchical idea to construct the Voronoi_Obstacle field and uses a heuristic search algorithm to plan the path. The specific steps are as follows:

[0006] S1: Construct potential field based on obstacles;

[0007] S2: construct potential field based on Voronoi diagram;

[0008] S3: Establishing the Voronoi_Obstacle field based on the obstacle potential field and the Voronoi diagram potential field;

[0009] S4: Heuristic path planning search algorithm based on Voronoi_Obstacle field.

[0010] As a preferred embodiment of the present invention, for S1, a map expansion method is used to plan a safe path away from obstacles by setting an expansion value, thereby reducing the risk of collision. The potential field of obstacles is established by using the expansion idea, and the actual size of the mobile robot is considered. The potential field formula is as follows:

[0011]

[0012] Where: κ x is the danger value of the distance to the obstacle, which gradually tends to 0 as it moves away from the obstacle; x is a point in the configuration space, X is the configuration space of the robot, and p ob is the nearest obstacle at x, d(x,p ob ) is x and p ob The distance, D robot is the size of the robot, and μ is the decay rate.

[0013] As a preferred embodiment of the present invention, for S2, the plane is divided into sub-regions based on a specific point subset, the specific point subset is pre-specified, wherein the specific point subset is a seed, and the distance from any point in the sub-region corresponding to each seed to the seed is closer than the distance to any other seed, and the Voronoi diagram is defined as follows:

[0014] R k ={x∈X|d(x,P k )≤d(x,P j )for all j≠k}

[0015] Where: R k is the Voronoi region, P k , P j For a specific tuple, d(x,P k) is x to P k The Voronoi potential field is established for the generated Voronoi diagram using the expansion idea. The Voronoi potential field does not consider the actual size of the robot. The potential field formula is as follows:

[0016]

[0017] Where: x ∈[0,1), indicating the degree of distance from Voronoi, which gradually tends to 0 as it moves away from Voronoi; p vo represents the Voronoi point closest to the robot; d(x,p vo ) for x to p vo distance; v is the gain rate.

[0018] As a preferred embodiment of the present invention, for S3, the Voronoi_Obstacle field is calculated using the hierarchical multi-step concept, and the calculation formula is as follows:

[0019]

[0020] In the formula: When κ x ≥κ max When x is 0, ρ x As it moves away from Voronoi, κ max is the maximum attenuation value, which is a constant value, regardless of the distance between the nearest obstacle and the nearest Voronoi point, and is calculated by κ x and χ x Calculate the Voronoi_Obstacle field. In two-dimensional space, the complexity of the Voronoi_Obstacle field is O(n 2 ), the complexity is reduced to O(n 0.5 ).

[0021] As a preferred embodiment of the present invention, for S4, a heuristic weight value is added on the basis of the A* algorithm, the weight of the heuristic value is artificially changed, and g(n) and h(n) are redefined, and the expression is as follows:

[0022] f(n)=g(n)+w·h(n)

[0023]

[0024] Where: f(n) is the evaluation function from the starting point to the target point, g(n) is the moving cost from the starting point to node n, h(n) is the estimated cost from node n to the target node, which represents the heuristic cost, w is the heuristic weight value, ρ iis the Voronoi field value at configuration space i, and d(x,y) represents the distance from x to y.

[0025] Compared with the prior art, the present invention has the following beneficial effects:

[0026] The present invention discloses a global path planning method based on Voronoi_Obstacle field, which is used for autonomous navigation of a mobile robot. Firstly, an upper-layer system provides scene information, planning starting point information and planning end point information for path planning. Then, a hierarchical concept is adopted to construct the Voronoi_Obstacle field. Under the premise of ensuring that the algorithm can obtain an optimal or suboptimal safe distance path, the running time of the algorithm is reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is a hierarchical structure diagram of the Voronoi_Obstacle field of the present invention;

[0028] Figure 2 It is the algorithm flow chart of the present invention;

[0029] Figure 3 The Voronoi_Obstacle field diagram of the present invention;

[0030] Figure 4 This is a path planning algorithm based on the Voronoi_Obstacle field of the present invention. DETAILED DESCRIPTION

[0031] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0032] See also Figure 1-4 The present invention provides a technical solution: a mobile robot path planning algorithm based on Voronoi_Obstacle field, which avoids the high calculation complexity and long calculation time of traditional Voronoi field, and the slow calculation of traditional path planning based on Voronoi diagram when the space becomes larger, the burrs increase, and the grid becomes denser.

[0033] In order to solve the above technical problems, the present invention adopts the following technical solution, comprising the following steps:

[0034] S1: Construct potential field based on obstacles;

[0035] S2: construct potential field based on Voronoi diagram;

[0036] S3: Establishing the Voronoi_Obstacle field based on the obstacle potential field and the Voronoi diagram potential field;

[0037] S4: Heuristic search algorithm based on Voronoi_Obstacle field;

[0038] For step S1, map expansion is widely used in mobile robot navigation. By setting a suitable expansion value, a safe path away from obstacles can be planned to reduce the risk of collision. This patent uses the expansion idea to establish the potential field of obstacles. Considering the actual size of the mobile robot, the potential field formula is as follows:

[0039]

[0040] Where: κ x is the danger value of the distance to the obstacle, which gradually tends to 0 as it moves away from the obstacle; x is a point in the configuration space; X is the configuration space of the robot; p ob is the nearest obstacle at x; d(x,p ob ) is x and p ob Distance; D robot is the size of the robot; μ is the decay rate.

[0041] For step S2, the Voronoi diagram divides the plane into sub-regions based on a specific subset of points, where the specific subset of points is a seed and is pre-specified, and the distance from any point in the sub-region corresponding to each seed to the seed is closer than the distance to any other seed. The definition of the Voronoi diagram is as follows:

[0042] R k ={x∈X|d(x,P k )≤d(x,P j )for all j≠k}

[0043] Where: R k is the Voronoi region; P k , P j is a specific tuple, namely the seed; d(x,P k ) is x to P k The distance is consistent with the S1 algorithm idea. This patent uses the expansion idea to establish the Voronoi potential field for the generated Voronoi diagram. The Voronoi potential field does not consider the actual size of the robot. The potential field formula is as follows:

[0044]

[0045] Where: x∈[0,1), indicating the degree of distance from Voronoi, which gradually tends to 0 as it moves away from Voronoi; p vo represents the Voronoi point closest to the robot; d(x,p vo ) for x to p vo distance; v is the gain rate.

[0046] For step S3, the traditional Voronoi field is calculated as follows:

[0047]

[0048] In the formula: When V (x,y) is 0, ρ V (x,y) will gradually approach 1 as it moves away from the Voronoi direction; d O (x,y) is the distance from the path point (x,y) to the nearest obstacle; d V (x,y) is the distance from the path point (x,y) to the nearest Voronoi; α is the attenuation rate; is the maximum attenuation distance, the complexity of the traditional Voronoi field is O(n 2.5 ), which is highly complex and takes a long time to calculate. To solve this problem, this patent proposes to use a hierarchical multi-step approach to calculate the Voronoi_Obstacle field. The calculation formula is as follows:

[0049]

[0050] In the formula: When κ x ≥κ max When x is 0, ρ x It will gradually approach 1 as it moves away from Voronoi; κ max is the maximum attenuation value, which is a constant value. The above formula does not consider the distance between the nearest obstacle and the nearest Voronoi point, but is calculated through κ x and χ x Calculate the Voronoi_Obstacle field. In two-dimensional space, the complexity of the Voronoi_Obstacle field proposed in this patent is O(n 2 ), the complexity is reduced to O(n 0.5 );

[0051] The detailed hierarchical structure of the Voronoi_Obstacle field based on the obstacle potential field and the Voronoi diagram potential field is as follows: Figure 2 The hierarchical structure of the Voronoi_Obstacle field is shown in the figure.

[0052] For step S4, the A* algorithm is a typical heuristic search calculation method, and its expression is as follows:

[0053] f(n)=g(n)+h(n)

[0054] Where: f(n) is the evaluation function from the starting point to the target point; g(n) is the moving cost from the starting point to node n; h(n) is the estimated cost from node n to the target node, which represents the heuristic cost;

[0055] This patent uses an improved A* algorithm to perform heuristic search on the Voronoi_Obstacle field. On the basis of A*, a heuristic weight value is added, the weight of the heuristic value can be changed artificially, and g(n) and h(n) are redefined. The expressions are as follows:

[0056] f(n)=g(n)+w·h(n)

[0057]

[0058] Where: w is the heuristic weight value; ρ i is the Voronoi field value at configuration space i; d(x,y) represents the distance from x to y. The algorithm flow is as follows Figure 3 shown.

[0059] In order to verify the beneficial effect of the implementation in the path search stage compared with the prior art, the present invention establishes a simulated obstacle map on the ROS Rviz platform, which is based on the Voronoi_Obstacle field of the obstacle potential field and the Voronoi diagram potential field. Figure 4 As shown in , the global path planned is as follows Figure 4 As shown in the figure, an optimal or suboptimal safety distance path can be obtained, while reducing the running time of the algorithm.

[0060] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A mobile robot path planning algorithm based on Voronoi_Obstacle field, characterized by: The Voronoi_Obstacle field is constructed using a hierarchical concept, and the path is planned using a heuristic search algorithm. The specific steps are as follows: S1: Construct potential field based on obstacles; S2: construct potential field based on Voronoi diagram; S3: Establishing the Voronoi_Obstacle field based on the obstacle potential field and the Voronoi diagram potential field; S4: Heuristic path planning search algorithm based on Voronoi_Obstacle field.

2. The mobile robot path planning algorithm based on Voronoi_Obstacle field according to claim 1, characterized in that: For S1, the map expansion method is used. By setting the expansion value, a safe path away from obstacles is planned to reduce the risk of collision. The expansion idea is used to establish the potential field of obstacles. Considering the actual size of the mobile robot, the potential field formula is as follows: Where: κ x is the danger value of the distance to the obstacle, which gradually tends to 0 as it moves away from the obstacle; x is a point in the configuration space, X is the configuration space of the robot, and p ob is the nearest obstacle at x, d(x,p ob ) is x and p ob The distance, D robot is the size of the robot, and μ is the decay rate.

3. The mobile robot path planning algorithm based on Voronoi_Obstacle field according to claim 1, characterized in that: For S2, the plane is divided into sub-regions based on a specific point subset, which is pre-specified, where the specific point subset is a seed, and the distance from any point in the sub-region corresponding to each seed to the seed is closer than the distance to any other seed. The Voronoi diagram is defined as follows: R k ={x∈X|d(x,P k )≤d(x,P j )for all j≠k} Where: R k is the Voronoi region, P k , P j For a specific tuple, d(x,P k ) is x to P k The Voronoi potential field is established for the generated Voronoi diagram using the expansion idea. The Voronoi potential field does not consider the actual size of the robot. The potential field formula is as follows: Where: x ∈[0,1), indicating the degree of distance from Voronoi, which gradually tends to 0 as it moves away from Voronoi; p vo represents the Voronoi point closest to the robot; d(x,p vo ) for x to p vo distance; v is the gain rate.

4. The mobile robot path planning algorithm based on Voronoi_Obstacle field according to claim 1, characterized in that: For S3, the idea of ​​hierarchical multi-step is used to calculate the Voronoi_Obstacle field. The calculation formula is as follows: In the formula: When κ x ≥κ max When x is 0, ρ x As it moves away from Voronoi, κ max is the maximum attenuation value, which is a constant value, regardless of the distance between the nearest obstacle and the nearest Voronoi point, and is calculated by κ x and χ x Calculate the Voronoi_Obstacle field. In two-dimensional space, the complexity of the Voronoi_Obstacle field is O(n 2 ), the complexity is reduced to O(n 0.5 ).

5. The mobile robot path planning algorithm based on Voronoi_Obstacle field according to claim 1, characterized in that: For S4, a heuristic weight value is added on the basis of the A* algorithm, the weight of the heuristic value is artificially changed, and g(n) and h(n) are redefined. The expressions are as follows: f(n)=g(n)+w·h(n) Where: f(n) is the evaluation function from the starting point to the target point, g(n) is the moving cost from the starting point to node n, h(n) is the estimated cost from node n to the target node, which represents the heuristic cost, w is the heuristic weight value, ρ i is the Voronoi field value at configuration space i, and d(x,y) represents the distance from x to y.

Images