Orchard robot path planning method based on improved Informed RRT*

Abstract

The invention discloses an orchard robot path planning method based on improved Informed RRT *, and the method comprises the steps: carrying out the obstacle collision detection between a pnearest and a pgoal through employing a greedy strategy; an improved artificial potential field method is adopted, so that the target point pgoal and the random point prand generate gravitation on the pnest, the obstacle generates repulsive force on the pnest, and a new node pnew of the step length d is generated in the resultant action direction of the gravitation and the repulsive force; updating the step length d by adopting a self-adaptive dynamic step length strategy; continuously searching for an optimal path in an elliptical sampling space by using an elliptical sampling strategy of Informed RRT *, and pruning the path after a new path is obtained each time by using a pruning strategy based on trisection points; and outputting the optimal path. According to the invention, the node generation randomness can be reduced, the path planning speed is improved, and the path planning length is reduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular relates to an orchard robot path planning method with improved Informed RRT*. Background Art

[0002] Orchard robots, as a key enabler of intelligent modern agriculture, play a crucial role in scenarios such as fruit picking and plant protection. These robots must achieve autonomous navigation within complex orchard environments, and efficient path planning is key. Path planning involves using algorithms to create an optimal trajectory for the robot from its starting point to its destination. This trajectory must simultaneously meet obstacle avoidance requirements and optimize operational efficiency. It is a fundamental technology supporting the precise operation of orchard robots.

[0003] The RRT algorithm and its improved methods, such as RRT* and Informed RRT*, are widely used in orchard robot path planning. The Informed RRT* algorithm utilizes ellipse heuristic sampling to accelerate convergence and achieve better planned paths. However, this algorithm still suffers from issues such as high randomness in generated nodes, a large number of generated nodes, lengthy path planning times, and a non-optimal generated path. Summary of the Invention

[0004] The purpose of the present invention is to overcome the shortcomings of the prior art and provide an orchard robot path planning method based on an improved Informed RRT*, which effectively reduces the randomness of node generation, improves the path planning speed, and reduces the length of the planned path.

[0005] The purpose of the present invention is achieved through the following technical solutions:

[0006] An orchard robot path planning method based on improved Informed RRT* includes the following steps:

[0007] (1) Under the condition of obtaining global information, initialize the starting point p start 、Target point p goal And the obstacle coordinates, and add the safety distance to expand the obstacle and initialize the algorithm parameters;

[0008] (2) At the starting point p start and the target point p goal Random sampling is performed in the rectangular sampling space formed to obtain a random point p rand , find the distance p rand The nearest node p nearest ;

[0009] (3) Using the greedy strategy, p nearest With p goalObstacle collision detection is performed between the two objects. If there is no collision, the p nearest Connect to p goal Get the initial path, otherwise continue with the new node generation step;

[0010] (4) Using the improved artificial potential field method, the target point p goal and a random point p rand For p nearest Generates gravitational force, the obstacle has an impact on p nearest Generate repulsion and generate a new node p with a step length of d in the direction of the combined force of attraction and repulsion new ;

[0011] (5) After each iteration, the step size d is updated using an adaptive dynamic step size strategy;

[0012] (6) If p new The distance from the target point is less than the threshold T goal , and node p new and the target point p goal When the line between them does not collide with obstacles, connect p new and p goal Generate the initial path, otherwise return to step (2);

[0013] (7) After obtaining the initial path, the elliptical sampling strategy of Informed RRT* is used to continue searching for the optimal path in the elliptical sampling space, and the pruning strategy based on the trisection point is used to prune the path each time a new path is obtained;

[0014] (8) If the maximum number of iterations is reached (the default initial value of the maximum number of iterations is 200), the final path is output and the algorithm ends. Otherwise, return to step (7) to continue searching for the optimal path.

[0015] In step (3), a greedy strategy is adopted to obtain a random point p in each iteration. rand Then, get the distance p rand The nearest node p nearest After that, p nearest and p goal Perform obstacle detection and connect p if there is no obstacle blocking nearest and p goal Get the initial path, otherwise continue to the new node generation step.

[0016] In step (4), the improved artificial potential field method comprises the following steps:

[0017] (4-1) Calculate the random point p rand and the target point p goal For p nearestThe resulting gravitational field function is shown below:

[0018] U att (p) = w rand U att_rand (p)+w goal U att_goal (p) (1)

[0019]

[0020] Among them, U att (p) is p rand and p goal P nearest The total attractive force after weighted attraction, U att_rand (p) is p rand P nearest The resulting gravitational field function, U att_goal (p) is p goal P nearest The resulting gravitational field function, k att is the attraction field gain coefficient, ρ(p,p rand ) is p rand to p nearest The distance, ρ(p,p goal ) is p goal to p nearest distance; perform negative gradient operation on the gravitational field function to obtain the gravitational force F att_rand (p) and F att_goal (p), as shown below:

[0021] F att_rand (p) = k att ρ(p,p rand ) (4)

[0022] F att_goal (p) = k att ρ(p,p goal ) (5)

[0023] F att_rand (p) and F att_goal (p) respectively assign weight coefficient w rand and w goal , set the initial value w rand =0.1, w goal =0.9;

[0024] P nearest With p goal Perform obstacle detection. When there is an obstacle and the obstacle distance is p nearest Less than the threshold T ob When rand=0.9, w goal =0.1;

[0025] The total force of attraction F att (p) is calculated as follows:

[0026] F att (p) = w rand F att_rand (p)+w goal F att_goal (p) (6)

[0027] (4-2) The obstacle to p nearest The resulting repulsive field function is:

[0028]

[0029] Among them, k rep is the repulsive field gain function, ρ(p,p obs ) is the distance from the obstacle to p nearest ρ0 is the distance affected by the obstacle;

[0030] Performing a negative gradient operation on the repulsive field function, we obtain the repulsive force calculation formula:

[0031]

[0032] (4-3) The formula for calculating the resultant force F(p) of repulsion and attraction is as follows:

[0033] F(p)=F att (p)+F rep (p) (9).

[0034] In step (5), the adaptive dynamic step size strategy is used to update the step size d. The dynamic step size update mechanism based on the sliding window collision probability is adopted. The window size parameter is set to W. After the algorithm starts, it is recorded whether the new node generated in each iteration collides with the obstacle. When the number of iterations is greater than W, the probability r of the new node generated in the W iterations before the current iteration collides with the obstacle is calculated according to formula (10): col , and calculate the updated step size according to formula (11):

[0035]

[0036]

[0037] Among them, n col is the number of collisions in the W iterations before the current iteration, d0 is the current step size, d is the updated step size, k d is the step size adjustment coefficient, T col is the collision probability threshold.

[0038] In step (7), after generating the initial feasible path, if the number of path nodes is ≥3, the current path is pruned using the path pruning method based on the trisection point; if the number of path nodes is <3, no pruning is required. Specifically, the following steps are included:

[0039] (7-1) Starting from the target node, let the current node be p current , try to get its parent node p along the current path father and its grandparent node p grandfather ;

[0040] (7-2) When p grandfather When present, detect p current With p grandfather Whether the path directly connected between them collides with the obstacle, if there is no collision, p current The parent node is updated to p grandfather , end the pruning of the current node; if there is a collision, p grandfather With p father The path segment is divided into three equal parts, and the two equidistant dividing points are divided from p father to p grandfather The directions are set as child nodes Q1 and Q2 respectively, and their coordinate calculation formulas are:

[0041]

[0042] Among them, (x g ,y g ) is p grandfather Coordinates, (x f ,y f ) is p father coordinate;

[0043] Sequentially connect child nodes Q1 and Q2 to p current Connect and perform obstacle collision detection. If there is no collision, set the child node to p current The parent node of the child node is set to p grandfather , until both child nodes are detected and the path is updated;

[0044] Similarly, p current With p father The path segments between them are divided into three equal parts, and the two equally spaced dividing points are divided into two equal parts according to the rule of p father to p current The directions are set as child nodes R1 and R2 respectively, and their coordinate calculation formulas are:

[0045]

[0046] Among them, (xc ,y c ) is p current Coordinates, (x f ,y f ) is p father coordinate;

[0047] Sequentially connect child nodes R1 and R2 to p grandfather Connect and perform obstacle collision detection. If there is no collision, set the child node to p current The parent node of the child node is set to p grandfather , until both child nodes are detected and the path is updated.

[0048] Compared with the existing technology, the present invention has the following advantages and effects: In order to solve the problems of high randomness and slow path planning speed of common path planning methods in complex environments, the present invention adopts a greedy strategy, an improved artificial potential field method, a dynamic step size and a path pruning method to improve the Informed RRT*, which can reduce the randomness of node generation, improve the path planning speed and reduce the length of the planned path. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 Schematic diagram of the improved artificial potential field method guiding the growth of new nodes.

[0050] Figure 2 is the p in the pruning strategy current Try connecting directly to p grandfather Schematic diagram of pruning.

[0051] Figure 3 is the p in the pruning strategy current Schematic diagram of attempting to connect child nodes Q1 and Q2 for pruning.

[0052] Figure 4 is the p in the pruning strategy grandfather Schematic diagram of attempting to connect child nodes R1 and R2 for pruning.

[0053] Figure 5 Schematic diagram comparing the pruned path and the original path in the pruning strategy.

[0054] Figure 6 This is a schematic diagram of traditional Informed RRT* path planning.

[0055] Figure 7 This is a schematic diagram of the improved Informed RRT* path planning of the present invention. DETAILED DESCRIPTION

[0056] For ease of understanding of the present invention, the present invention will be described in detail below in conjunction with specific embodiments. The following examples will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be pointed out that, for those skilled in the art, without departing from the inventive concept, the present invention can also make several variations and improvements, which all fall within the scope of protection of the present invention.

[0057] Example 1

[0058] (1) Under the condition of obtaining global information, initialize the starting point p start is (0,0), target point p goal The coordinates of the map are in meters. The coordinates of the obstacles are initialized and the size of the obstacles after the safety distance is added to the expansion process is calculated. The algorithm parameters are initialized. The maximum number of iterations M is initialized to 200. The dynamic step size collision probability calculation window size W is 5. The initial step size is 2.0. The step size adjustment coefficient k d is 4.0, the collision probability threshold T col is 0.5, the obstacle influence range ρ0 is 3.0, and the attraction field gain coefficient k att is 5.0, the repulsive field gain coefficient k req is 3.0.

[0059] (2) At the starting point p start and the target point p goal Random sampling is performed in the rectangular sampling space formed to obtain a random point p rand , find the distance p rand The nearest node p nearest .

[0060] (3) Using the greedy strategy, p nearest With p goal Obstacle collision detection is performed between the two objects. If there is no collision, the p nearest Connect to p goal Get the initial path, otherwise continue with the new node generation step.

[0061] (4) Use the improved artificial potential field method to generate new nodes, so that the target point p goal and a random point p rand For p nearest Generates gravitational force, the obstacle acts on p nearest Generate repulsion and generate a new node p with a step length of d in the direction of the combined force of attraction and repulsion new .

[0062] like Figure 1 The following is an example of the improved artificial potential field method for generating new nodes, where p start is the starting point, pgoal is the target point, p rand A random point generated by random sampling, p nearest The current node tree and p rand The node with the smallest distance, the obstacle is the node with the smallest distance from p among all the current obstacles. nearest Obstacles whose distance is less than the obstacle impact threshold ρ0; because Figure 1 In the example shown, the current iteration p nearest With p goal There is an obstacle blocking the space between the two nearest The distance is less than the threshold T ob , the node growth direction needs to be more inclined towards p rand , so set the weight w rand is 0.9, w goal is 0.1, Figure 1 Medium F att_rand For p rand P nearest The resulting weighted attraction, F att_goal For p goal P nearest The resulting weighted attraction, F att F att_rand and F att_goal The resultant force, F req The obstacle is p nearest The repulsive force generated, F total F att and F req The direction of the resultant force is the direction in which the new node is generated.

[0063] (5) The step size d is updated using a dynamic step size update mechanism based on the sliding window collision probability. After the algorithm starts, it records whether a new node collides with an obstacle when it is generated in each iteration. The current window size W is 5. When the number of iterations is greater than 5, after each iteration, the probability r of a new node generated in the first 5 iterations of the current iteration colliding with an obstacle is calculated according to formula (10): col , and calculate the updated step size according to formula (11).

[0064] (6) If p new The distance from the target point is less than the threshold T goal , and node p new and the target point p goal When the line between them does not collide with obstacles, connect p new and p goal Generate an initial path, otherwise return to step (2).

[0065] (7) After obtaining the initial path, the elliptical sampling strategy of Informed RRT* is used to continue searching for the optimal path in the elliptical sampling space, and the pruning strategy based on the trisection point is used to prune the path each time a new path is obtained.

[0066] After generating the initial feasible path, if the number of path nodes is ≥3, the current path is pruned using the path pruning method based on the trisection point. If the number of path nodes is <3, no pruning is required.

[0067] like Figure 2 As shown, starting from the target node, let the current node be p current , try to get its parent node p along the current path father and its grandparent node p grandfather When p grandfather When present, detect p current With p grandfather Whether the path directly connected between them collides with the obstacle, if there is no collision, p current The parent node is updated to p grandfather , end the pruning of the current node. If there is a collision, such as Figure 3 As shown, p grandfather With p father The path segment is divided into three equal parts, and the two equidistant dividing points are divided from p father to p grandfather The direction of is set as child nodes Q1 and Q2, and the child nodes Q1 and Q2 are connected with pcurrent Connect and perform obstacle collision detection. If there is no collision, set the child node to p current The parent node of the child node is set to p grandfather , until both child nodes are detected and the path is updated.

[0068] like Figure 4 As shown, similarly, p current With p father The path segments between them are divided into three equal parts, and the two equally spaced dividing points are divided into two equal parts according to the rule of p father to p current Set the direction of the child nodes R1 and R2 in turn. grandfather Connect and perform obstacle collision detection. If there is no collision, set the child node to p current The parent node of the child node is set to p grandfather , until both child nodes are detected, the path comparison before and after pruning is as follows Figure 5 As shown, update the path.

[0069] (8) If the maximum number of iterations is reached, the final path is output and the algorithm ends. Otherwise, it returns to step (7) to continue searching for the optimal path. The final path output by the algorithm is as follows: Figure 7 Shown in red path.

[0070] In order to verify the effectiveness of the improved Informed RRT* algorithm of the present invention, the traditional Informed RRT* algorithm and the improved Informed RRT* algorithm of the present invention were simulated repeatedly 10 times. The simulation data are shown in Table 1.

[0071] Table 1 Algorithm comparison simulation data table

[0072]

[0073] As shown in Table 1, the improved Informed RRT* algorithm of the present invention has a significant improvement over the traditional Informed RRT* algorithm. Compared with the traditional Informed RRT* algorithm, the path length is reduced by 3.1% and the planning time is reduced by 84.3%.

[0074] like Figure 6 and Figure 7 These are the path planning diagrams of the traditional Informed RRT* algorithm and the improved Informed RRT* algorithm. It can be seen that compared with the traditional Informed RRT* algorithm, the improved Informed RRT* algorithm has the characteristics of reducing the number of sampling points, more purposeful node generation, and a shorter final path.

[0075] It can be understood that the above specific description of the present invention is only used to illustrate the present invention and is not limited to the technical solutions described in the embodiments of the present invention. Ordinary technicians in this field should understand that the present invention can still be partially modified or replaced with equivalents to achieve the same technical effects; as long as the use requirements are met, they are within the scope of protection of the present invention.

Claims

1. An improved Informed RRT* orchard robot path planning method, characterized by The steps include: (1) Under the condition of obtaining global information, initialize the starting point p start 、Target point p goal And the obstacle coordinates, and add the safety distance to expand the obstacle and initialize the algorithm parameters; (2) At the starting point p start and the target point p goal Random sampling is performed in the rectangular sampling space formed to obtain a random point p rand , find the distance p rand The nearest node p nearest ; (3) Using the greedy strategy, nearest With p goal Obstacle collision detection is performed between the two objects. If there is no collision, the p nearest Connect to p goal Get the initial path, otherwise continue with the new node generation step; (4) Using the improved artificial potential field method, the target point p goal and a random point p rand For p nearest Generates gravitational force, the obstacle has an impact on p nearest Generate repulsion and generate a new node p with a step length of d in the direction of the combined force of attraction and repulsion new ; (5) After each iteration, the step size d is updated using an adaptive dynamic step size strategy; (6) If p new The distance from the target point is less than the threshold T goal , and node p new and the target point p goal When the line between them does not collide with obstacles, connect p new and p goal Generate the initial path, otherwise return to step (2); (7) After obtaining the initial path, the elliptical sampling strategy of Informed RRT* is used to continue searching for the optimal path in the elliptical sampling space, and the pruning strategy based on the trisection point is used to prune the path each time a new path is obtained; (8) If the maximum number of iterations is reached, the final path is output and the algorithm ends; otherwise, it returns to step (7) to continue searching for the optimal path.

2. The improved Informed RRT* orchard robot path planning method according to claim 1, characterized in that: In step (3), a greedy strategy is adopted to obtain a random point p in each iteration. rand Then, get the distance p rand The nearest node p nearest After that, p nearest and p goal Perform obstacle detection and connect p if there is no obstacle blocking nearest and p goal Get the initial path, otherwise continue to the new node generation step.

3. The improved Informed RRT* orchard robot path planning method according to claim 1, characterized in that: In step (4), the improved artificial potential field method comprises the following steps: (4-1) Calculate the random point p rand and the target point p goal For p nearest The resulting gravitational field function is shown below: U att (p)=w rand U att_rand (p)+w goal U att_goal (p) (1) Among them, U att (p) is p rand and p goal P nearest The total attractive force after weighted attraction, U att_rand (p) is p rand P nearest The resulting gravitational field function, U att_goal (p) is p goal P nearest The resulting gravitational field function, k att is the attraction field gain coefficient, ρ(p,p rand ) is p rand to p nearest The distance, ρ(p,p goal ) is p goal to p nearest distance; perform negative gradient operation on the gravitational field function to obtain the gravitational force F att_rand (p) and F att_goal (p), as shown below: F att_rand (p)6k att ρ(p,p rand ) (4) F att_goal (p)6k att ρ(p,p goal ) (5) F att_rand (p) and F att_goal (p) respectively assign weight coefficient w rand and w goal , set the initial value w rand =0.1, w goal =0.9; P nearest With p goal Perform obstacle detection. When there is an obstacle and the obstacle distance is p nearest Less than the threshold T ob When rand =0.9, w goal =0.1; The total force of attraction F att (p) is calculated as follows: F att (p)=w rand F att_rand (p)+w goal F att_goal (p) (6) (4-2) The obstacle to p nearest The resulting repulsive field function is: Among them, k rep is the repulsive field gain function, ρ(p,p obs ) is the distance from the obstacle to p nearest ρ0 is the distance affected by the obstacle; Performing a negative gradient operation on the repulsive field function, we obtain the repulsive force calculation formula: (4-3) The formula for calculating the resultant force F(p) of repulsion and attraction is as follows: F(p)=F att (p)+F rep (p) (9)。 4. The improved Informed RRT* orchard robot path planning method according to claim 1, characterized in that: In step (5), the adaptive dynamic step size strategy is used to update the step size d. The dynamic step size update mechanism based on the sliding window collision probability is adopted. The window size parameter is set to W. After the algorithm starts, it is recorded whether the new node generated in each iteration collides with the obstacle. When the number of iterations is greater than W, the probability r of the new node generated in the W iterations before the current iteration collides with the obstacle is calculated according to formula (10): col , and calculate the updated step size according to formula (11): Among them, n col is the number of collisions in the W iterations before the current iteration, d0 is the current step size, d is the updated step size, k d is the step size adjustment coefficient, T col is the collision probability threshold.

5. The improved Informed RRT* orchard robot path planning method according to claim 1, characterized in that: In step (7), after generating the initial feasible path, if the number of path nodes is ≥3, the current path is pruned using the path pruning method based on the trisection point; if the number of path nodes is <3, no pruning is required. Specifically, the following steps are included: (7-1) Starting from the target node, let the current node be p current , try to get its parent node p along the current path father and its grandparent node p grandfather ; (7-2) When p grandfather When present, detect p current With p grandfather Whether the path directly connected between them collides with the obstacle, if there is no collision, p current The parent node is updated to p grandfather , end the pruning of the current node; if there is a collision, p grandfather With p father The path segment is divided into three equal parts, and the two equidistant dividing points are divided from p father to p grandfather The directions are set as child nodes Q1 and Q2 respectively, and their coordinate calculation formulas are: Among them, (x g ,y g ) is p grandfather Coordinates, (x f ,y f ) is p father coordinate; Sequentially connect child nodes Q1 and Q2 to p current Connect and perform obstacle collision detection. If there is no collision, set the child node to p current The parent node of the child node is set to p grandfather , until both child nodes are detected and the path is updated; Similarly, p current With p father The path segments between them are divided into three equal parts, and the two equally spaced dividing points are divided into two equal parts according to the rule of p father to p current The directions are set as child nodes R1 and R2 respectively, and their coordinate calculation formulas are: Among them, (x c ,y c ) is p current Coordinates, (x f ,y f ) is p father coordinate; Sequentially connect child nodes R1 and R2 to p grandfather Connect and perform obstacle collision detection. If there is no collision, set the child node to p current The parent node of the child node is set to p grandfather , until both child nodes are detected and the path is updated.

Images