Global-local path planning method adaptive to fruit tree inter-row operation

Abstract

The invention belongs to the technical field of mobile robot path planning, and provides a global-local path planning method adaptive to fruit tree inter-row operation. According to the method, a dynamic weight mechanism is introduced into an A * algorithm in global path planning, and the weight of a heuristic function is adaptively adjusted according to two factors, namely the ratio of the distance from a node to a target point and the transverse spacing of fruit trees, so that the search precision is improved; a * algorithm is utilized to pre-plan and construct a real cost matrix which is used for guiding segmented optimization of the ant colony algorithm, meanwhile, an adaptive pheromone volatilization mechanism based on a solution improvement rate is adopted to improve convergence stability, and path smoothing is realized in combination with redundant point deletion. In local planning, a path deviation cost item is introduced into a dynamic window method to quantify trajectory deviation, and a dynamic weighting strategy is adopted to adaptively adjust the weight of each evaluation function according to the obstacle distance. According to the method, efficient, smooth and stable path planning can be realized in a complex orchard environment, and the method is suitable for a mobile robot for fruit tree row-to-row operation.

Description

Technical Field

[0001] This invention relates to the field of robot path planning technology, specifically a global-local path planning method adapted for inter-row operations in fruit trees. Background Technology

[0002] With the rapid development of intelligent and automated agricultural technologies, mobile robots are increasingly used in various operational scenarios in orchards, such as harvesting, spraying, inspection, and transportation. They play a crucial role in improving operational efficiency, reducing labor costs, and enhancing operational safety. However, orchard operations have significant unique characteristics: fruit trees are densely distributed in rows (row spacing 2-3m, plant spacing 1.5-2m), the passageways between rows are narrow and have varying slopes, and dynamic obstacles (fallen fruit, moving farm tools, and workers) appear randomly. This leads to three core pain points in traditional path planning algorithms: 1) The A* algorithm generates too many path turns, making it prone to collisions with the tree trunks; 2) The ant colony algorithm converges slowly in the search of sub-regions between rows, resulting in strong initial path blindness; 3) The Dynamic Window (DWA) method tends to deviate from the global path between rows during local obstacle avoidance, getting trapped in local optima between fruit trees and obstacles, making it difficult to meet the needs of efficient and safe operation of orchard robots. Therefore, how to achieve efficient and safe path planning in a highly structured yet uncertain environment like an orchard has become an important direction and key technical challenge in the research of intelligent orchard robots in recent years.

[0003] Existing path planning algorithms mainly include A* algorithm and ant colony algorithm. While A* algorithm offers fast search speed, it tends to generate paths with too many turns in the confined space of orchards. Ant colony algorithm, although possessing global optimization capabilities, suffers from slow convergence speed and initial blind search. Furthermore, traditional dynamic window methods are prone to getting stuck in local optima during obstacle avoidance, making them unsuitable for real-time operations in dynamic orchard environments. Although existing hybrid algorithms have made some progress in path planning, they still suffer from problems such as non-adaptive search strategies, fixed pheromone update mechanisms, and a lack of global path guidance for local obstacle avoidance in specific orchard scenarios, leaving significant room for improvement in path quality and planning efficiency. Summary of the Invention

[0004] To address the problems existing in the prior art, this invention provides a global-local path planning method adapted for orchard row operations. Targeting the characteristics of orchard row spacing constraints and the random distribution of dynamic obstacles, it achieves efficient path planning through four core innovations: 1) an improved two-factor dynamic weighting mechanism for the A* algorithm, adapting to fast and accurate searching in narrow rows; 2) a sub-region cost matrix based on A* pre-planning to solve the blindness problem in ant colony algorithm row searching; 3) an adaptive pheromone evaporation mechanism with improved rate quantization to enhance path convergence stability; and 4) an improved DWA algorithm for global path guidance to avoid local obstacle avoidance deviations from the row path, ultimately achieving efficient, smooth, and safe path planning for the orchard robot.

[0005] To achieve the above-mentioned objectives, the technical solution adopted by this invention is a global-local path planning method adapted for inter-row operations in fruit trees, comprising the following steps: Step S1: Collect orchard map data and construct a 0.1m×0.1m grid map with a slope cost layer by combining the 2-3m row spacing of fruit trees (cost ×2 for slope > 15°, additional passage difficulty cost coefficient + 0.8 when slope > 25°). Mark obstacles such as fruit trees, fallen fruits (diameter ≥ 0.1m) and passable areas between rows (width ≥ 1.2m, adapted to the width of the robot body).

[0006] Step S2: In the global planning, the improved A* is adjusted with the heuristic weights of "target distance ratio + fruit tree horizontal spacing" (1.2-1.5 for wide spacing for long distances and 0.6-0.9 for narrow spacing for short distances); the area is divided into sub-regions according to the fruit tree row spacing of 2.2-2.8m, and an actual cost matrix including turning (60°→0.3, 90°→0.5, 120°→0.8, 180°→1.2) and distance (Euclidean × 1.2 + inter-row passage correction coefficient 0.3) is constructed to guide the ant colony to find the best solution in segments; the ant colony adjusts the pheromone evaporation coefficient according to the solution improvement rate (5 generations ≥15%→0.1-0.2, 5%-15%→0.3-0.4, 8 generations <5%→0.6-0.7); redundant points are deleted with "3° collinearity + no collision", key nodes are retained, and the path turning angle is ≤15°.

[0007] Step S3: In local planning, DWA adds a global path deviation cost (average distance of 30 sampling points, >0.5m triggers correction); adjusts the weights according to obstacle threat level (<1m high / 1-2m medium / >2m low) and target proximity (>10m far / 5-10m medium / <5m near) (high threat obstacle avoidance ≥60%, long distance high speed ≥50%, close distance azimuth angle ≥55%) to achieve obstacle avoidance and accurate positioning.

[0008] The beneficial effects of the global-local path planning method adapted for inter-row operations in fruit trees proposed in this invention are as follows: (1) In terms of global path planning, compared with the traditional ant colony algorithm, in the simulated orchard 20×20m grid map (including 8 fruit tree obstacles), the number of convergence iterations was reduced from 66 to 25 (a reduction of 62.1%), the path length was shortened by 2.6%, and the number of path turns was reduced by 35%, which is suitable for the smooth movement requirements between fruit tree rows; in the 30×30m grid map (including 12 fruit tree obstacles), the number of convergence iterations was reduced by 67.4%, the path length was shortened by 12.87%, and the planning time was shortened by 45.5% (7.61s→4.15s). (2) In terms of local path planning, the search time of the improved DWA algorithm is reduced by 26.3% (174.87s→128.88s), the number of path nodes is reduced by 28, and the deviation of the global path after obstacle avoidance is ≤0.3m. It successfully solves the problem that traditional algorithms are prone to getting stuck in local optima in the dynamic environment of orchards, and is suitable for various inter-row operation scenarios such as picking and spraying. Attached Figure Description

[0009] Figure 1 This is an overall flowchart of a global-local path planning method adapted for inter-row operations of fruit trees according to the present invention; Figure 2 This is a simulation result of the traditional ant colony algorithm on a 30×30 grid map; Figure 3 This is a simulation result of the improved ant colony algorithm for a 30×30 grid map; Figure 4 This is a simulation result diagram using the traditional dynamic window method; Figure 5 This is a simulation result diagram of the improved dynamic window method. Detailed Implementation

[0010] To make the objectives, technical solutions, and advantages of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0011] like Figure 1 As shown, a global-local path planning method adapted for inter-row operations in fruit trees is implemented with the following steps: Step S1: Obtain orchard environment information, select 20×20 grid map and 30×30 grid map as orchard environment simulation scene, mark obstacle area and passable area, set grid map resolution to 0.1m×0.1m, construct slope cost layer (slope > 15° cost × 2), mark 8 / 12 fruit tree obstacles with 2-3m row spacing.

[0012] Step S2: Input the processed map into the modified global path planning algorithm for path planning and optimization. First, the improved A* algorithm introduces a two-factor dynamic weight of "target distance ratio + fruit tree lateral spacing" (with a distance-to-width ratio of 1.3) for preliminary planning, identifying key turning points of the path; sub-regions are divided according to a 2.5m row spacing, and an actual cost matrix is ​​constructed including turning cost (90° + 0.5) and distance cost (Euclidean × 1.2); an adaptive ant colony algorithm (30 ants, maximum iteration 100) is used to dynamically adjust the pheromone evaporation coefficient according to the solution improvement rate; finally, redundant points are deleted using a 3° collinearity deviation angle as the threshold through a linkage strategy of "three-point collinearity determination + line segment obstacle collision detection".

[0013] Step S3: Perform simulation experiments and compare the results using the method described in Step S2. The simulation results are as follows: Figure 2 and Figure 3 As shown, in the 20×20 map, the number of convergence iterations decreased from 66 to 25, a reduction of 62.1%, the path length was shortened by 2.6%, and the planning time was reduced from 7.61 seconds to 4.15 seconds, a reduction of 45.5%; in the 30×30 map, the number of iterations decreased from 46 to 15, a reduction of 67.4%, and the path length was shortened by 12.87%. Detailed Implementation Method 2

[0014] To make the objectives, technical solutions, and advantages of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0015] like Figure 1 As shown, a global-local path planning method adapted for inter-row operations in fruit trees is implemented with the following steps: Step S1: Set up a dynamic obstacle avoidance test scenario in a 20×20 grid map to simulate the actual situation of falling fruit and moving farm tools in orchard operations. The dynamic obstacle moves along the positive direction of the horizontal coordinate axis at a speed of 0.2m / s. The robot's initial position is (1, 1), and the target position is (20, 20).

[0016] Step S2: Construct an improved dynamic window method evaluation function. Based on the traditional dynamic window method evaluation function, a path deviation cost evaluation term is introduced. The degree of deviation is quantified by calculating the average shortest distance between the predicted trajectory and the global reference path. The weight coefficients of each evaluation term are dynamically adjusted: when the obstacle distance is <1m (high threat), the safety obstacle avoidance weight accounts for 65%; when the obstacle distance is >2m and the target point distance is >10m (long distance), the speed weight accounts for 55%; when the target point distance is <5m (near distance), the azimuth weight accounts for 58%.

[0017] Step S3: Perform simulation experiments and compare the results using the method described in Step S2. The simulation results are as follows: Figure 4 and Figure 5 As shown. Compared to the traditional dynamic window method, the improved dynamic window method reduces the number of nodes by 28, shortens the search time by 26.3%, significantly improves the efficiency of path search, and also produces a smoother attitude angle curve during movement.

[0018] To verify the effectiveness of the global-local path planning method for fruit tree row operations provided by this invention, the present invention also provides the following experiments.

[0019] (1) Experimental data and experimental environment This invention uses a 20×20 grid map for testing, which contains randomly distributed obstacles to simulate an orchard environment. The experiment involved placing eight static obstacles (4 rows and 2 columns, row spacing 2.5m) and one dynamic obstacle (simulating falling fruit) on the 20×20m grid map to conduct local obstacle avoidance tests.

[0020] The experiment was simulated using MATLAB R2020a, with a Windows 11 operating system and an Intel Core i7 processor.

[0021] (2) Experimental setup and evaluation indicators The improved dynamic window method was selected for testing local path planning. The dynamic window method parameters were set as follows: velocity sampling resolution of 0.01 m / s, angular velocity sampling resolution of 0.1 rad / s, and prediction time window of 3 s. The improved dynamic window method introduces a path deviation cost evaluation term and employs a dynamic weighting coefficient adjustment strategy.

[0022] The following metrics are used to evaluate the performance of local path planning: search time, final path length, and number of nodes traversed.

[0023] (3) Experimental results and analysis The dynamic obstacle avoidance scenario in the 20×20 grid map was tested using the traditional dynamic window method and the improved dynamic window method proposed in this invention. The results are shown in Table 1.

[0024] Table 1 Comparison of Dynamic Window Method Before and After Improvement Algorithm Name Search time / s Path length Number of nodes Traditional DWA algorithm 174.87 28.06 385 Improved DWA algorithm 128.88 27.95 357 Combination Figure 5 As shown in Table 1, the improved dynamic window method has a shorter path length than the traditional dynamic window method, reduces the number of nodes by 28, and shortens the search time by 26.3%, significantly improving the efficiency of path search. Furthermore, the attitude angle curve during movement is smoother. This set of experiments demonstrates that the improved dynamic window method not only outperforms the traditional dynamic window method in path search and obstacle avoidance capabilities, but also surpasses it in search time, path length, and number of control nodes, further illustrating the effectiveness and superiority of the improved algorithm.

Claims

1. A global-local path planning method adapted for inter-row operations in fruit trees, characterized in that, The method includes the following steps: S1: Collect orchard map data and, combined with tree row spacing constraints (2-3m) and trunk diameter (0.3-0.5m), construct a 0.1m×0.1m grid map model with an orchard-specific slope cost layer: when the slope is >15°, the cost coefficient is increased by 2 times; when the slope is >25°, an additional passage difficulty cost (coefficient +0.8) is added; mark obstacle areas such as tree trunks, fallen fruits (diameter ≥0.1m), and moving farm tools, and ensure that the width of the passable area between rows is ≥1.2m (adapted to the robot's body width); S2: Construct a global path planning module and improve the A* algorithm by introducing a two-factor dynamic weighting mechanism of "node-target distance ratio + lateral safety spacing of fruit trees (≥0.8m)" to adjust the weight of the heuristic function; pre-plan the global path by improving the A* algorithm, divide the inter-row sub-regions with the inter-row spacing of fruit trees (2.2-2.8m) as the dynamic benchmark, and construct the actual reachability cost matrix for avoiding fruit trees between sub-regions (including turning cost: 0.5 for 90° turning, 1.2 for 180° turning; distance cost: Euclidean distance × 1.2 + inter-row passage correction coefficient 0.3) to guide the ant colony algorithm for segmented optimization; embed an adaptive pheromone evaporation mechanism based on the solution improvement rate quantization threshold into the ant colony algorithm to optimize path convergence; adopt the linkage strategy of "three-point collinearity judgment (deviation angle ≤3°) + line segment-fruit tree collision detection" to delete redundant points and ensure that the path turning angle is ≤15° (adapting to the minimum turning radius of the orchard robot ≥0.5m). S3: Construct a local path planning module with global-local collaborative feedback. Add a global path deviation cost evaluation term (sampling point density 0.01m, adapted to a working speed of 0.1m / s) to the Dynamic Window Method (DWA) evaluation function. Design a dynamic weighting coefficient adjustment strategy based on orchard obstacle characteristics to adaptively optimize the weights of each evaluation function. The global path feeds back a reference trajectory to the local module every 50ms. After local obstacle avoidance, regression is triggered by the deviation cost term (threshold 0.5m) to ensure that the path conforms to the row distribution of fruit trees.

2. The method according to claim 1, characterized in that, In step S2, the two-factor dynamic weighting mechanism of the improved A* algorithm is used to adaptively adjust the heuristic function weights based on the ratio of the distance from the node to the target point and the lateral safety distance from the node to the fruit trees on both sides: when the distance from the node to the target point is greater than 50% of the length of the map diagonal and the lateral safety distance is greater than or equal to 1.5m, the weight coefficient is set to 1.3-1.5 to speed up the inter-row search; when the distance from the node to the target point is less than or equal to 30% of the length of the map diagonal or the lateral safety distance is less than 1.0m, the weight coefficient is set to 0.6-0.8 to improve the trunk obstacle avoidance accuracy.

3. The method according to claim 1, characterized in that, The method for constructing the actual cost matrix in step S2 is as follows: A global path is pre-planned using an improved A* algorithm. The row spacing (2.2-2.8m) is used as a dynamic benchmark to divide the area into inter-row sub-regions, with the sub-region boundaries parallel to the centerline of the fruit tree rows. The actual reachability cost of bypassing fruit trees between sub-regions is calculated. This cost includes turning cost and distance cost. The turning cost increases with the turning angle (0.3 for 60° turns, 0.5 for 90° turns, and 0.8 for 120° turns). The distance cost is Euclidean distance × 1.2 plus an inter-row flatness correction coefficient (≤5° slope → 0.1, 5-15° slope → 0.2, >15° slope → 0.3), forming a two-dimensional actual cost matrix. This replaces the unconstrained cost evaluation based solely on Euclidean distance in the traditional ant colony algorithm.

4. The method according to claim 1, characterized in that, In step S2, the adaptive pheromone volatilization mechanism dynamically adjusts the volatilization coefficient based on the solution improvement rate quantification threshold of the orchard path optimization characteristics: when the solution improvement rate is ≥15% for 5 consecutive generations (significant improvement), the volatilization coefficient is set to 0.12-0.18 to accelerate the convergence speed; when the solution improvement rate is between 5% and 15% (small improvement), the volatilization coefficient is set to 0.32-0.38 to balance global exploration and local development; when the solution improvement rate is <5% for 8 consecutive generations (search stagnation), the volatilization coefficient is set to 0.62-0.68 to break the local optimum and avoid premature convergence to the non-optimal path between fruit tree rows.

5. The method according to claim 1, characterized in that, In step S2, the redundant point deletion strategy adopts a linkage mechanism of "three-point collinearity judgment + line segment-fruit tree collision detection": when the collinearity deviation angle of three consecutive nodes is ≤3°, and the minimum distance between the line connecting the three points and the trunk of the fruit tree is ≥0.3m (adapted to the diameter of the fruit tree trunk), and there is no collision with static obstacles, the intermediate node is deleted; the first and last nodes of the path, the turning points of the fruit tree and the turning nodes between the rows of fruit trees are retained to ensure that the turning angle of the path after processing is ≤15°.

6. The method according to claim 1, characterized in that, In step S3, the path deviation cost evaluation item is quantified by calculating the average shortest distance between 30 equidistant sampling points on the predicted trajectory and the global reference path (the sampling point density is set according to the robot's working speed of 0.1m / s (sampling interval 0.01m)). When the deviation distance is >0.5m, trajectory correction is triggered to guide the robot back to the global path between the fruit tree rows.

7. The method according to claim 1, characterized in that, In step S3, the dynamic weighted coefficient adjustment strategy is adaptively adjusted based on the obstacle threat level and target proximity based on the orchard obstacle characteristic classification: obstacle threat level is classified by distance (distance <1m is high threat, 1-2m is medium threat, and >2m is low threat), and target proximity is classified by distance (distance >10m is far, 5-10m is medium, and <5m is near); when the threat is high, the safety obstacle avoidance weight accounts for ≥65%; when the threat is low and the target is far away, the speed weight accounts for ≥55%; when the target is near, the azimuth weight accounts for ≥58%, ensuring accurate positioning of operations between fruit tree rows.

Images