Full-coverage low-power-consumption path planning method for fruit-bearing forests in hilly and mountainous regions
By constructing an energy consumption model and map decomposition, a low-power consumption path that meets the needs of fruit forest coverage and tree-wrapped operations in hilly and mountainous areas is planned, which solves the problems of high energy consumption and incomplete coverage in the existing technology, and achieves the safe and efficient completion of fruit forest operations.
Patent Information
- Application Number
- CN202510635326.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-05-16
- Publication Date
- 2025-08-15
AI Technical Summary
The existing path planning methods cannot meet the needs of covering and tree-surrounding fruit trees in hilly and mountain fruit forests, and do not consider energy consumption, resulting in high energy consumption and low safety in operation machinery under complex terrain.
By constructing an energy consumption model, combining the fruit forest elevation map and planting information, map classification and area decomposition are carried out, potential entry points and optimal traversal order are determined, low-power consumption paths that meet the coverage in the fruit forest are planned, and path smoothing is performed to reduce energy consumption.
The full coverage path planning of fruit forest operation machinery is realized, which reduces redundant energy consumption, improves operation safety and efficiency, and ensures the integrity and low power consumption characteristics of fruit forest management operations.
Smart Images
Figure CN120489157A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of unmanned vehicle navigation path planning, and in particular to a full-coverage, low-power consumption path planning method for fruit forests in hilly and mountainous areas. Background Art
[0002] Fruit plantation is a cyclical operation that requires repeated tasks such as forest tending, tree management, and fruit harvesting. Due to the complex hilly and mountainous terrain, manual work in the orchards is labor-intensive and unsafe. While a variety of forestry machinery exists to meet diverse operational needs, alleviating the burden on workers to some extent, these machines lack intelligence and still require manual operation and remote control. Therefore, a path planning algorithm is needed to enable autonomous movement of these machines in complex terrain.
[0003] The hilly and mountainous areas where fruit trees are planted have complex terrain, divided into flat areas, sloping areas, and concave areas based on slope gradients. Because fruit trees lack the ability to recharge or refuel machinery, it's crucial to analyze the varying terrain conditions and the energy consumption of machinery, explore the most energy-efficient travel methods, and plan the most energy-efficient routes through the forest to avoid unnecessary energy loss for machinery.
[0004] Unlike field planting, fruit trees present obstacles that cannot be traversed by machinery and equipment. They also pose challenges for forest tending operations, such as weeding and loosening tree pits, pruning, and fruit harvesting. Furthermore, some orchards have large spacing between rows, so path planning within orchards must ensure coverage not only within the forest path but also between and around the trees.
[0005] Currently, common path planning methods are used in open terrain with few, large obstacles. Obstacle avoidance or obstacle-based zone division can be used to achieve full coverage. However, these methods cannot meet the requirements of inter-row and tree-circling operations. Fruit trees can even hinder the path planning logic, making them unsuitable for fruit orchards. Most path planning methods for agricultural operations fail to consider energy consumption, focusing instead on finding the shortest traversal path. For operations in completely flat terrain, the energy consumption of the machine is related to the path length, so the shortest path is the path with the lowest energy consumption. However, in hilly and mountainous terrain, due to the need for climbing and descending, the energy consumption of the machine is dependent on multiple parameters such as machine weight, climbing height, and path length. Therefore, a corresponding energy consumption model is needed to calculate the path with the lowest energy consumption. Therefore, a low-power path planning method is needed for hilly and mountainous fruit orchards that meets the requirements of inter-row coverage and tree-circling operations. Summary of the Invention Technical issues solved: In order to achieve autonomous and complete coverage of hilly and mountainous fruit forest areas by forest operation tools, avoid missing operations, reduce redundant energy consumption, save operator workload, and ensure operational safety, the purpose of the present invention is to provide a full-coverage, low-power path planning method for hilly and mountainous fruit forests, combining the constraints of the geometric parameters and operation parameters of the operation tools, and comprehensively considering the distribution of different terrain types and the planting conditions of fruit trees, to plan a forest path that meets the forest roads, inter-plant and around-tree operations in the fruit forest, without missing operations and with the lowest energy consumption. The present invention plans a reasonable path, assists the operation machinery to travel flexibly, smoothly, safely and autonomously between the trees, completes various fruit forest management operations or harvesting operations, and reduces the energy loss of redundant tools at the same time.
[0006] The present invention achieves the above objectives through the following technical solutions: The present invention provides a full-coverage, low-power consumption path planning method for fruit forests in hilly and mountainous areas.
[0007] A low-power, full-coverage path planning method for orchards in hilly and mountainous areas requires orchard elevation map information of the known operating environment, orchard planting information, and mechanical parameters of the operating tools. A low-power full-coverage path planning method for hilly orchards includes the following implementation steps: Determine the energy consumption model based on the mechanical parameters of the work tool; Read the orchard elevation map information and orchard planting information, and build a unit grid type map information model; According to the map information model, the map is classified and decomposed into regions; According to the regional decomposition results, the potential entry points and taboo entrances of each sub-unit are determined, and the optimal regional traversal order and the most suitable entry point are calculated and screened; According to the area traversal order, low-power full coverage path planning is carried out for each sub-unit in the area in turn; According to the regional traversal order and the starting and ending points of the traversal path within each sub-unit, the sub-units are connected to complete the full coverage path planning of the entire orchard operation area and calculate the related energy consumption; Smoothing the planned path based on machine performance to avoid unnecessary stops and turns, and reducing additional energy consumption; Based on the map information model and the obtained planned path, the path planning results are visualized.
[0008] Preferably, determining the energy consumption model according to the mechanical parameters of the work tool includes: Based on the mass of the working tool and the winding resistance coefficient of the travel system on the sandy loam surface, the force analysis on the inclined surface is carried out. The arbitrary heading angle of the tool is set, and the relationship between the force on the tool and the heading angle and the inclination angle of the inclined surface is constructed; The energy consumption composition is analyzed based on the walking status of the machine in the hilly and mountainous fruit forest. In order to avoid the problem of the calculated full coverage path planning path of the fruit forest being disorganized and falling into the local optimal problem, the operation area needs to be partitioned to obtain multiple sub-units. According to the scope and traversal order of the sub-units, the internal coverage of each sub-unit and the path connection between sub-units are completed in sequence to achieve full coverage of the operation area. Therefore, the total energy consumption of the machine is divided into energy consumption within the area, energy consumption for connection between areas, and energy consumption for operation; Energy consumption varies depending on the task. For example, consider weeding, which is essential in forests. Because weeding energy consumption is solely dependent on the weeding motor or engine, and independent of terrain or tool parameters, the power and, therefore, energy consumption, can be calculated based on the speed.
[0009] Preferably, reading the orchard elevation map information and the orchard planting information and constructing a unit grid type map information model includes: Set the step length for reading elevation data based on the tool size parameters. This step length is the side length of the unit grid. To avoid missing work areas, the side length of the unit grid needs to be smaller than the tool size. Decompose the orchard operation area into a unit grid. Read the elevation map data set (world coordinate system) according to the set step size, and determine the total size of the unit grid based on the data set size (row × column). Store the row and column coordinate information of the elevation data into the corresponding unit grid. The grid cell states are set based on the orchard planting information. The initial state of each grid cell is divided into a passable area, a fruit tree, and an obstacle. During path planning, the state of the grid cell can be transformed from a passable area to a covered area.
[0010] The elevation data of the cell is integrated with the status data to complete the map information modeling.
[0011] Preferably, according to the map information model, classifying and decomposing the map accordingly includes: In order to prevent the algorithm from being overly sensitive to elevation information, frequently adjusting the path direction, and increasing extra energy consumption, an elevation threshold is set. When the elevation value of a grid cell differs from the elevation values of the upper, lower, left, and right adjacent grid cells by more than the threshold, the grid cell is classified as a non-flat terrain cell, including slopes and pits; the remaining grid cells with elevation values less than the threshold are classified as flat terrain cells. According to the grid coordinate position of the non-flat terrain unit, the K-Means clustering algorithm is used to classify the unit grid into regions and generate non-flat terrain sub-units. The number of sub-units formed by the final classification is determined by the number of non-flat terrains. Due to the irregularities of non-flat terrain boundaries, in order to facilitate regional decomposition of the entire orchard operation area, a rectangular expansion process is performed on each non-flat terrain sub-unit in the classification results. The maximum and minimum values of the row and column coordinates within the sub-unit are extracted. All cell grids in the map whose coordinates are within the maximum range of the row and column are attributed to the current sub-unit, and the coordinates composed of the minimum and maximum values of the row and column represent the area of the sub-unit. According to the distribution of non-flat terrain sub-units, the entire operation area is decomposed with reference to the principle of flood diversion and merging, that is, the water flow is diverted when encountering an obstacle and merged after passing the obstacle. When decomposing the area, the non-flat terrain sub-units are treated as obstacles and the flat land units are partitioned. Along the row width direction of the operation area, the left and right boundary column coordinates of all non-flat terrain sub-units after rectangular expansion are extracted, and the four corner points of all sub-units are classified according to their column coordinates. Each category of corner points with the same vertical coordinate constitutes a dividing line. The dividing lines are sorted in ascending order of column coordinates, and the corner points on each dividing line are sorted in ascending order of row coordinates; Insert the intersection points of each dividing line with the upper and lower boundaries of the work area at the beginning and end points. If the corner point already exists on a dividing line, it does not need to be inserted again. To properly handle the situation where the column coordinates of multiple sub-units on non-flat terrain partially overlap, the row coordinates of the upper and lower boundaries of the sub-units are inserted sequentially on the dividing line between the left and right boundaries of the sub-units. After passing through the sub-unit, the area becomes connected, so it is necessary to remove the corner points on the dividing line that belong to the right boundary of the sub-unit; Sequentially extract two corner points on a certain dividing line. Under the condition that there is no non-flat terrain sub-unit between the two points, the horizontal area between the two corner points and the next dividing line is a flat terrain sub-unit. It is important to note that the operation is not repeated for each corner point. Mark the sub-unit area according to the representation method of the non-flat terrain sub-unit; The left boundary of the orchard operation area and the first dividing line, as well as the right boundary of the orchard operation area and the last dividing line are respectively used as the first and last flat land terrain sub-units.
[0012] Preferably, according to the regional decomposition results, the potential entry points and taboo entrances of each sub-unit are determined, and the optimal regional traversal order and the optimal entry points are calculated and screened, including: Subunits and their entry points are encoded in two levels. The first level uses "T+i" or "P+j" (i, j = 1, 2, 3, ..., n) to represent each subunit, where "T" represents non-flat terrain, "P" represents flat terrain, and n is the number of subunits in each type of terrain. The second level uses binary encoding. Since each subunit has a maximum of four potential entry points, only one entry point is selected, and each entry point is represented using two bits of binary code, i.e., "00, 01, 10, 11." Therefore, the subunit encoding format should be "Ti-00" or "Pj-00." The order of the entry point codes corresponds to the entry point's position as follows: 00 (0) - top right corner, 01 (1) - top left corner, 10 (2) - bottom left corner, and 11 (3) - bottom right corner.
[0013] Using a region decomposition algorithm, multiple rectangular region sub-units are obtained. Based on the status values of the four corner points of each sub-unit, the grid cells that are obstacles or fruit trees are excluded. These grid cells are uniformly stored in a taboo search list and memorized for a long time to prevent them from being mistakenly selected as region entry points.
[0014] According to the number of sub-units, a reasonable number of iterations and solution set size are set, and multiple groups of traversal order and entry point combinations are randomly generated as the initial solution set. The number of traversal orders is the solution set size.
[0015] Construct a fitness function. The fitness function is the inverse of the Euclidean distance between the exit of the subunit traversal path and the entry point of the next traversal subunit.
[0016] Construct the expected exit calculation function. Under the requirement of minimum power consumption, select the "snake path" of the long side scanning as the basic path for the sub-unit, and calculate the exit point of the basic path based on the parity of the long side size of each sub-unit.
[0017] The fitness of each solution in the initial solution set is calculated, and the solution with the maximum fitness is selected as the temporary optimal solution. It is stored in the taboo search list for long-term memory to avoid subsequent redundant generation of the same solution and increase the diversity of solutions.
[0018] The first-level encoding of the parent solution set undergoes a 2-opt operation. This involves randomly selecting two subunits within a given traversal order, swapping their positions within the solution, and reversing the order of the remaining subunits between them. The second-level encoding performs a mutation operation, generating multiple possible solutions as the child solution set. For each solution in the child solution set, the exit points corresponding to each subunit are calculated, and fitness is calculated. The solution with the highest fitness is selected as the temporary optimal solution and stored in a tabu search list for long-term memory. This prevents redundant generation of the same solution and increases solution diversity. This step is repeated until the number of iterations is reached or until fitness has not significantly improved. The optimal solution is output as the region traversal order and the entry point of each subunit.
[0019] Preferably, performing low-power full coverage path planning within the area for each sub-unit in turn according to the area traversal order includes: Decode the optimal solution. Read the two-layer code in the optimal solution in sequence and decompose it. Decode the first layer to obtain the corresponding sub-unit terrain type and the coordinate range of the sub-unit corresponding grid; decode the second layer, convert the binary to decimal, obtain the corresponding entry point order, and then obtain the row and column coordinates of the entry point based on this order. For flat terrain subunits, the total distance of the snake traversal along the long side or the short side is the same, so the long side traversal is chosen to reduce the number of turns and thus reduce energy loss. For non-flat terrain units, a greedy selection algorithm based on minimum energy consumption is used to plan a full coverage path. Eight points around the current grid are selected to perform local slope fitting on the slope to obtain the approximate inclination angle of the slope relative to the horizontal plane. Based on parameters such as the inclination angle, equipment weight, and vehicle winding resistance coefficient, the energy consumption of the four adjacent points above, below, left, and right of the grid is calculated. The grid that meets the requirements of being passable, uncovered, and having the lowest energy consumption is selected as the next path. Therefore, the planned path is composed of each grid as a path point. In the process of fully covering the path of flat terrain units and non-flat terrain units, when the next grid state is a tree, the embedded tree-circling direction determination algorithm is entered. When the fruit tree is at the boundary of the sub-unit, the grid that is perpendicular to the path forward direction and is not covered is selected as the next grid, and the grid is not marked as covered to avoid disrupting the subsequent path planning; in order to ensure that the direction of the overall path is not destroyed, it is necessary to adjust the next grid to return to the original forward direction in time, so executing the tree-circling direction determination algorithm once will determine the next two grids. For the fruit tree grid inside the sub-unit, the grid that is perpendicular to the path forward direction and has been covered is selected as the next grid, and the path is adjusted back to the original forward direction through the next grid; When no four-way grids adjacent to the current grid are passable and uncovered, the path is considered to be in a dead zone. The optimized A* algorithm is used to escape the dead zone, with the target grid determined by the shortest Euclidean distance. Since a grid in the escape path may no longer be in a dead zone, escape path planning is stopped and that grid is replaced as the target grid, avoiding a lengthy escape path and increasing energy consumption. The A* search algorithm, commonly known as the A-star algorithm, is a popular heuristic search algorithm widely used in path optimization.
[0020] Preferably, according to the regional traversal order and the starting point and end point of the traversal path in each sub-unit, the sub-units are connected to complete the full coverage path planning of the entire orchard operation area, and the relevant energy consumption is calculated including: Based on the global map information, after traversing all sub-units, the endpoint of each sub-unit's internal path and the starting point of the next sub-unit's internal path are sequentially read, and the A* algorithm is used to plan the shortest obstacle-free path between them. Due to the expansion of non-flat terrain units, the entry and exit points of all sub-units are located on flat terrain. To avoid unnecessary gravitational work loss during the area connection process, all connection paths are located on flat terrain. Based on these settings, the connection path with the lowest energy consumption between areas is the shortest connection path.
[0021] Preferably, the planned path is smoothed according to the performance of the machine to avoid unnecessary stops and turns and reduce additional energy consumption, including: Path smoothing is performed based on the turning radius parameters of the machine. For machines with a turning radius of 0, inscribed arc segments are constructed at the path turns and U-turns for smoothing. For machines with a turning radius not equal to 0, fishtail, bow, or pear shapes are selected based on the relationship between the working width and the turning radius. When the turning radius is no greater than half the working width, a bow shape is selected; when the turning radius is greater than half the working width, a pear or fishtail shape is selected. Based on the selected path smoothing scheme and the planned path, a specific path point and two subsequent path points are selected at a time to determine the change in the machine's heading angle. When the path turns or U-turns, the arc's center, tangent point, and arc length are calculated based on the geometric relationships defined by the smoothing scheme. Since the path is stored as discrete points, 100 points along the arc length are selected as path points for smoothing. If the path remains straight, only that point is used as a path point for the smoothed path, and the remaining two points are re-evaluated. This operation is repeated for each path point in the unsmoothed path to obtain a smoothed path.
[0022] Preferably, visualizing the path planning result according to the map information model and the obtained planned path includes: Based on the constructed map information model, a three-dimensional grid of the elevation map is generated. Different elevation grids are covered with different colors to reflect the characteristics of each terrain, and red dots are used to represent fruit tree planting information. On the constructed 3D map, each smoothed path point and its corresponding elevation information is sequentially read and connected with straight lines to display the planned smoothed path on the map. Since the number of path points selected for the arc segment is large, the visual interface approximates the arc. BRIEF DESCRIPTION OF THE DRAWINGS
[0023] Figure 1 This is an operational flow chart of a full-coverage, low-power path planning method for hilly and mountainous fruit forests; Figure 2 It is the force condition of the machine working in the orchard when it is traveling on a slope with a certain heading angle; Figure 3 It is a schematic diagram of the classification of sub-units of non-flat terrain within the orchard operation area, the processing of dividing lines, the zoning of the operation area, and the second-level coding of the sub-unit area; Figure 4 Different tree-circling principles are applied for fruit trees at the boundary of a region and those within the region; Figure 5 The operation process of dividing flat terrain subunits is based on the principle of flood diversion and confluence; Figure 6 Determine the optimal subunit traversal order and entry point operation flow chart for the genetic algorithm; Figure 7 Create a single child solution (not a solution set) schematic of the subunit traversal order and entry points. DETAILED DESCRIPTION
[0024] The technical solution of the present invention will be further described below with reference to the accompanying drawings and through specific implementation methods.
[0025] A method for planning a full-coverage, low-power path for a hilly orchard includes the following steps: S10. Determine an energy consumption model according to mechanical parameters of the work tool.
[0026] S20: Read the orchard elevation map information and the orchard planting information, and construct a unit grid type map information model.
[0027] S30: Classify and decompose the map into regions according to the map information model.
[0028] S40. According to the regional decomposition result, determine the potential entry points and taboo entrances of each sub-unit, and calculate and screen out the optimal regional traversal order and the most suitable entry point.
[0029] S50 , performing low-power full coverage path planning within the area for each sub-unit in turn according to the area traversal order.
[0030] S60: Connect the subunits according to the area traversal order and the starting point and end point of the traversal path in each subunit, complete the full coverage path planning of the entire orchard operation area, and calculate the relevant energy consumption.
[0031] S70. Smoothing the planned path according to the performance of the machine to avoid unnecessary stoppages and steering, thereby reducing additional energy consumption.
[0032] S80: Visualize the path planning result according to the map information model and the obtained planned path.
[0033] The step S10 further includes: S11. Analyze the forces acting on the inclined surface based on the mass of the work tool and the rolling resistance coefficient of the travel system on the sandy loam surface. Set the tool's arbitrary heading angle and construct a relationship between the force acting on the tool and the heading angle and the inclination angle of the inclined surface.
[0034] S12. Analyze the energy consumption components based on the machine's movement status in a hilly orchard. To avoid a disorganized path planning for full orchard coverage, which could lead to local optima, the operating area needs to be partitioned into multiple sub-units. Based on the sub-unit's scope and traversal order, the internal coverage of each sub-unit and the path connection between sub-units are completed sequentially to achieve full coverage of the operating area. Therefore, the machine's total energy consumption is divided into intra-regional energy consumption, inter-regional connection energy consumption, and operational energy consumption.
[0035] S13. Energy consumption varies depending on the task. For example, consider weeding, which is essential in forests. Because weeding energy consumption is solely dependent on the weeding motor or engine, and unrelated to terrain parameters or machine parameters, the weeding power, and thus energy consumption, can be calculated based on the speed.
[0036] The step S20 further includes: S21. Set the step length for reading elevation data based on the tool size parameters. This step length is the unit grid side length. To avoid missing work areas, the unit grid side length needs to be smaller than the tool size.
[0037] S22. Decompose the orchard operation area into a unit grid. Read the elevation map data set (world coordinate system) according to the set step size. Determine the total unit grid size based on the data set size (rows x columns). Store the row and column coordinates of the elevation data into the corresponding unit grid.
[0038] S23. Set the unit grid state according to the fruit forest planting information. The initial state of each grid is divided into a passable area, a fruit tree, and an obstacle. During path planning, the state of the unit grid can be converted from a passable area to a covered area.
[0039] S24: Fuse the cell information in S22 and S23 to complete the map information modeling.
[0040] The step S30 further includes: S31. To prevent the algorithm from being overly sensitive to elevation information, frequently adjusting the path direction, and increasing energy consumption, an elevation threshold is set. When the elevation value of a grid cell differs from the elevation values of the upper, lower, left, and right adjacent grid cells by more than the threshold, the grid cell is classified as a non-flat terrain cell, including slopes and pits. All other grid cells with elevation values less than the threshold are classified as flat terrain cells.
[0041] S32. Based on the grid coordinate positions of the non-flat terrain units, the K-Means clustering algorithm is used to perform regional classification on the unit grid to generate non-flat terrain sub-units. The number of sub-units formed by the final classification is determined by the amount of non-flat terrain.
[0042] S33. Due to the irregularity of the boundaries of non-flat terrain, to facilitate regional decomposition of the entire orchard operation area, a rectangular dilation process is performed on each non-flat terrain sub-unit in the classification results. The maximum and minimum values of the row and column coordinates within the sub-unit are extracted. All cell grids in the map whose coordinates fall within the maximum range of the row and column values are assigned to the current sub-unit, and the area of the sub-unit is represented by the coordinates consisting of the minimum row and column values and the coordinates consisting of the maximum row and column values.
[0043] S34. Decompose the entire operating area based on the distribution of non-flat terrain sub-units and the principle of flood diversion and merging, that is, the water flow diverts when encountering an obstacle and merges after passing the obstacle. When decomposing the area, treat the non-flat terrain sub-units as obstacles and partition the flat land units. Extract the left and right boundary column coordinates of all non-flat terrain sub-units after rectangular expansion along the row width direction of the operating area, classify the four corner points of all sub-units according to their column coordinates, and each category of corner points with the same vertical coordinate constitutes a dividing line. Sort the dividing lines in ascending order of column coordinates, and sort the corner points on each dividing line in ascending order of row coordinates.
[0044] S35. Insert the intersection points of each dividing line with the upper and lower boundaries of the work area at the beginning and end points. If the corner point already exists on a dividing line, it is not necessary to insert it again. To properly handle the situation where the column coordinates of multiple sub-units on non-flat terrain partially overlap, the row coordinates of the upper and lower boundaries of the sub-units are sequentially inserted onto the dividing line between the left and right boundaries of the sub-units. After passing through the sub-unit, the area becomes connected, so it is necessary to remove the corner points on the dividing line that belong to the right boundary of the sub-unit.
[0045] S36. Sequentially extract two corner points on a certain dividing line. Ensure that there is no non-flat terrain sub-unit between the two points. The horizontal area between the two corner points and the next dividing line is a flat terrain sub-unit. Note that the operation is not repeated for each corner point. Mark the sub-unit area according to the representation method of the non-flat terrain sub-unit.
[0046] S37, the left boundary of the orchard operation area and the first dividing line, and the right boundary of the orchard operation area and the last dividing line are respectively used as the first and last flat land terrain sub-units.
[0047] The step S40 further includes: S41. Perform two-level encoding on the subunits and their entry points. The first level of encoding uses "T+i" or "P+j" (i, j = 1, 2, 3, ..., n) to represent each subunit, where "T" represents non-flat terrain, "P" represents flat terrain, and n is the number of subunits in each type of terrain. The second level of encoding uses binary encoding. Since each subunit has a maximum of four potential entry points, only one entry point is selected, and each entry point is represented using two binary bits, i.e., "00, 01, 10, 11." Therefore, the subunit encoding format should be "Ti-00" or "Pj-00." The order of the entry point codes corresponds to the entry point's orientation as follows: 00 (0) - top right corner, 01 (1) - top left corner, 10 (2) - bottom left corner, and 11 (3) - bottom right corner.
[0048] S42. A plurality of rectangular region sub-units are obtained by using a region decomposition algorithm. Based on the status values of the four corner points of each sub-unit, the unit grids that are obstacles or fruit trees are excluded, and such grids are uniformly stored in a taboo search list and memorized for a long time to prevent such points from being mistakenly selected as region entry points.
[0049] S43. Set a reasonable number of iterations and solution set size according to the number of sub-units, and randomly generate multiple groups of traversal order and entry point combinations as the initial solution set. The number of traversal orders is the solution set size.
[0050] S44: Construct a fitness function. The fitness function is the inverse of the Euclidean distance between the exit of a subunit traversal path and the entry point of the next traversal subunit.
[0051] S45, constructing an expected exit calculation function. Under the requirement of minimum power consumption, a "snake path" of long-side scanning is selected as a basic path for the subunits, and the exit point of the basic path is calculated according to the parity of the long side size of each subunit.
[0052] S46. Calculate the fitness of each solution in the initial solution set, select the solution with the maximum fitness as the temporary optimal solution, and store it in the taboo search list for long-term memory to avoid subsequent redundant generation of the same solution and increase the diversity of solutions.
[0053] S47: Perform 2-opt processing on the first-level code of the parent solution set. This involves randomly selecting two subunits from a certain traversal order, swapping their positions in the solution, and reversing the order of the remaining subunits between them. The second-level code performs mutation processing to generate multiple possible solutions as the child solution set. For each solution in the child solution set, perform fitness calculations (S45). The solution with the highest fitness is selected as the temporary optimal solution and stored in the tabu search list for long-term memory. This prevents redundant generation of identical solutions and increases solution diversity.
[0054] S48. Repeat step S47 until the number of iterations or the fitness is not significantly improved, then stop the loop and output the optimal solution as the region traversal order and the entry point of each sub-unit.
[0055] The step S50 further includes: S51. Decode the optimal solution. Read the two-layer code in the optimal solution sequentially and decompose it. Decode the first layer to obtain the corresponding subunit terrain type and the coordinate range of the subunit's corresponding grid. Decode the second layer, convert the binary code to decimal, and obtain the corresponding entry point sequence. Then, based on this sequence, obtain the row and column coordinates of the entry points.
[0056] S52. For the flat terrain sub-unit, the total distance of the "snake" traversal along the long side or the short side is the same, so the traversal along the long side is selected to reduce the number of turns and thus reduce energy loss.
[0057] S53. For non-flat terrain cells, a greedy selection algorithm based on minimum energy consumption is used to plan a fully covered path. Eight points surrounding the current grid are selected to perform local slope fitting on the slope, obtaining an approximate slope angle relative to the horizontal plane. Based on parameters such as the slope angle, equipment weight, and vehicle winding resistance coefficient, the energy consumption of the four adjacent points above, below, left, and right of the grid is calculated. The grid that satisfies the requirements of being accessible, uncovered, and having the lowest energy consumption is selected as the next path. Therefore, the planned path is composed of each grid as a path point.
[0058] S54. During the full coverage path process for flat terrain units and non-flat terrain units, when the next grid state is a tree, the embedded tree-circling direction determination algorithm is entered. When the fruit tree is at the boundary of the sub-unit, the grid that is perpendicular to the path forward direction and is not covered is selected as the next grid, and the grid is not marked as covered to avoid disrupting the subsequent path planning; in order to ensure that the direction of the overall path is not destroyed, it is necessary to adjust the next grid in time to return to the original forward direction, so executing the tree-circling direction determination algorithm once will determine the next two grids. For the fruit tree grid inside the sub-unit, the grid that is perpendicular to the path forward direction and has been covered is selected as the next grid, and the path is adjusted back to the original forward direction through the next grid.
[0059] S55. When no four-way grids adjacent to the current grid are passable and uncovered, the path is determined to be in a dead zone. An optimized A* algorithm is used to escape the dead zone, with the target grid determined by the shortest Euclidean distance. Since a grid in the escape path may already be in a non-dead zone state, escape path planning is terminated in this case and that grid is replaced as the target grid, avoiding a lengthy escape path and increasing energy consumption.
[0060] The step S60 further includes: S61. Based on the global map information, after traversing all sub-units, the endpoint of each sub-unit's internal path and the starting point of the next sub-unit's internal path are sequentially read, and the A* algorithm is used to plan the shortest obstacle-free path between them. Due to the expansion of non-flat terrain units, the entry and exit points of all sub-units are located on flat terrain. To avoid unnecessary gravitational work loss during the area connection process, all connection paths are located on flat terrain. Based on the above settings, the connection path with the lowest energy consumption between areas is the shortest connection path.
[0061] The step S70 further includes: S71. Perform path smoothing based on the turning radius parameters of the machine. For machines with a turning radius of zero, smoothing is performed by constructing inscribed arc segments at path turns and U-turns. For machines with a turning radius greater than zero, a fishtail, bow, or pear shape is selected based on the relationship between the working width and the turning radius. A bow shape is selected when the turning radius is less than half the working width; a pear or fishtail shape is selected when the turning radius is greater than half the working width.
[0062] S72. Based on the selected path smoothing scheme and the planned path, select a path point and two subsequent path points at a time to determine the change in the machine's heading angle. If the path turns or U-turns, calculate the arc's center, tangent point, and arc length based on the geometric relationships in the smoothing scheme. Since the path is stored as discrete points, select 100 points along the arc length as path points after smoothing. If the path remains straight, only that point is used as a path point for the smoothed path, and the remaining two points need to be re-evaluated.
[0063] S73. Perform S72 on each path point in the non-smooth path to obtain a smooth path.
[0064] The step S80 further includes: S81. Generate a three-dimensional grid of the elevation map based on the map information model constructed in S20. Cover the grids with different colors for different elevation values to reflect the characteristics of each terrain. Use red dots to represent fruit tree planting information.
[0065] S82. On the constructed three-dimensional map, each smoothed path point and its corresponding elevation information are sequentially read and connected with straight lines, so that the planned smoothed path can be displayed on the map. Since the number of path points selected for the arc segment is large, the arc is approximated in the visualization interface.
[0066] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.
Claims
1. A method for planning a full-coverage, low-power path for a hilly orchard, characterized in that: The following steps are involved: S10, determining an energy consumption model based on mechanical parameters of the work tool; S20, reading the orchard elevation map information and the orchard planting information, and constructing a unit grid type map information model; S30, classifying and decomposing the map in sequence according to the map information model; S40, based on the regional decomposition results, determining the potential entry points and taboo entrances of each sub-unit, and calculating and screening the optimal regional traversal order and the most suitable entry point through genetic algorithm; S50, performing low-power full coverage path planning within the area for each sub-unit in turn according to the area traversal order; S60: Connect the subunits according to the area traversal order and the starting and ending points of the traversal paths within each subunit, complete the full coverage path planning for the entire orchard operation area, and calculate the relevant energy consumption; S70: Smoothing the planned path based on the machine performance to avoid unnecessary stoppages and diversions, thereby reducing extra energy consumption. S80: Visualize the path planning result according to the map information model and the obtained planned path.
2. The path planning method according to claim 1, characterized in that: The step S10 further includes: S11. Based on the mass of the work tool and the winding resistance coefficient of the travel system on the sandy loam surface, analyze the force on the inclined surface, set any heading angle of the tool, and construct a relationship between the force on the tool and the heading angle and the inclination angle of the inclined surface; S12. Analyze the energy consumption composition based on the walking status of the machine in the hilly and mountainous fruit forest, partition the operation area, and obtain multiple sub-units. According to the range and traversal order of the sub-units, complete the internal coverage of each sub-unit and the path connection between the sub-units in turn. The total energy consumption of the machine is divided into intra-regional energy consumption, inter-regional connection energy consumption, and operation energy consumption.
3. The path planning method according to claim 1, wherein: The step S20 further includes: S21. Set the step length for reading elevation data according to the tool size parameter. The step length is the side length of the unit grid. The side length of the unit grid needs to be smaller than the tool size. S22, the orchard operation area is decomposed into a unit grid, the elevation map data group is read according to the set step size, the total size of the unit grid is determined according to the data group size, and the row and column coordinate information of the elevation data is stored in the corresponding unit grid; S23. Setting the state of a unit grid according to the fruit forest planting information. The initial state of each grid is divided into a passable area, a fruit tree, and an obstacle. During path planning, the state of the unit grid can be converted from a passable area to a covered area. S24: Fuse the cell information in S22 and S23 to complete the map information modeling.
4. The path planning method according to claim 1, wherein: The step S30 further includes: S31. To prevent the algorithm from being overly sensitive to elevation information and frequently adjusting the path direction, an elevation threshold is set. When the elevation value of a grid cell differs from the elevation values of its upper, lower, left, or right neighboring cells by more than the threshold, the grid cell is classified as a non-flat terrain cell. The remaining cells with elevation values less than the threshold are classified as flat terrain cells. S32, performing regional classification on the non-flat terrain unit grid using a K-Means clustering algorithm to generate non-flat terrain sub-units, wherein the number of sub-units formed by the final classification is determined by the amount of non-flat terrain; S33. Perform a rectangular dilation process on each non-flat terrain sub-unit in the classification result, extract the maximum and minimum values of the row and column coordinates within the sub-unit, and assign all grid cells in the map whose coordinates fall within the maximum range of the row and column values to the current sub-unit. The area of the current sub-unit is represented by the coordinates consisting of the minimum row and column values and the coordinates consisting of the maximum row and column values. S34. Decompose the entire operation area based on the distribution of the non-flat terrain sub-units. During the decomposition, treat the non-flat terrain sub-units as obstacles and partition the flat terrain units. Extract the left and right boundary column coordinates of all the non-flat terrain sub-units after the rectangular expansion along the row width of the operation area. Classify the four corner points of all sub-units according to their column coordinates. Each category of corner points with the same vertical coordinate forms a segmentation line. Sort the segmentation lines in ascending order of column coordinates, and sort the corner points on each segmentation line in ascending order of row coordinates. S35. Insert the intersection points of each dividing line with the upper and lower boundaries of the working area at the beginning and end points. If the corner point already exists on a dividing line, it is not necessary to insert it again. Insert the row coordinates of the upper and lower boundaries of the subunit in order on the dividing line between the left and right boundaries of the subunit. After passing through the subunit, the area becomes connected. Remove the corner point on the dividing line that belongs to the right boundary of the subunit; S36, sequentially extracting two corner points on a certain dividing line, ensuring that there is no non-flat terrain sub-unit between the two points, and the lateral area between the two corner points and the next dividing line is a flat terrain sub-unit, and marking the sub-unit area according to the representation method of the non-flat terrain sub-unit; S37, the left boundary of the orchard operation area and the first dividing line, and the right boundary of the orchard operation area and the last dividing line are respectively used as the first and last flat land terrain sub-units.
5. The path planning method according to claim 2, characterized in that: The step S40 further includes: S41, perform two-layer encoding on the subunit and its entry point; S42. Obtain multiple rectangular area sub-units using a region decomposition algorithm. Eliminate unit grids that are obstacles or fruit trees based on the status values of the four corner points of each sub-unit. Store such unit grids in a taboo search list and memorize them for a long time to prevent them from being mistakenly selected as area entry points. S43. Setting a reasonable number of iterations and solution set size based on the number of subunits, randomly generating multiple sets of traversal order and entry point combinations as initial solution sets, where the number of traversal orders is the solution set size. S44, constructing a fitness function, where the fitness function is the inverse of the Euclidean distance between the exit of the subunit traversal path and the entry point of the next traversal subunit; S45. Construct an expected exit calculation function. Under the requirement of minimum power consumption, select a "serpentine path" of long-side scanning as the basic path for the subunits. Calculate the exit point of the basic path based on the parity of the long side size of each subunit. S46. Calculate the fitness of each solution in the initial solution set, select the solution with the maximum fitness as the temporary optimal solution, and store it in the taboo search list for long-term memory to avoid subsequent redundant generation of the same solution and increase the diversity of solutions; S47, perform 2-opt processing on the first layer code of the parent solution set, perform mutation processing on the second layer code, generate multiple possible solutions as the child solution set, and calculate the fitness of each solution in the child solution set; S48. Repeat step S47 until the number of iterations or the fitness is not significantly improved, then stop the loop and output the optimal solution as the region traversal order and the entry point of each sub-unit.
6. The path planning method according to claim 2, characterized in that: The step S50 further includes: S51. Decode the optimal solution. Read the two-layer code in the optimal solution in sequence and decompose the two-layer code. Decode the first layer to obtain the corresponding sub-unit terrain type and the coordinate range of the sub-unit corresponding grid. Decode the second layer, convert the binary code to decimal, obtain the corresponding entry point sequence, and then obtain the row and column coordinates of the entry point based on the sequence. S52. For the flat terrain sub-unit, if the total length of the traversal is the same as that of the traversal along the long side or the short side, the traversal along the long side is selected to reduce the number of turns and thus reduce energy loss. S53. For non-flat terrain cells, a greedy selection algorithm based on minimum energy consumption is used to perform full coverage path planning. Eight points surrounding the current grid are selected to perform local slope fitting on the slope surface, obtaining an approximate slope angle relative to the horizontal plane. Based on the slope angle, tool weight, and vehicle winding resistance coefficient parameters, the energy consumption of the four adjacent points above, below, to the left, and to the right of the grid is calculated. Among the four points, the grid that meets the requirements of being passable, uncovered, and having the lowest energy consumption is selected as the next path. The planned path is composed of path points in each grid. S54: When performing full coverage routing for flat and non-flat terrain cells, when the next grid state is a tree, the embedded tree-circling direction determination algorithm is entered. When a fruit tree is at the boundary of a sub-cell, an uncovered grid perpendicular to the path's forward direction is selected as the next grid, and the grid is not marked as covered to avoid disrupting subsequent path planning. To ensure that the overall path direction is not disrupted, the next grid needs to be adjusted in a timely manner to return to the original forward direction. S55. When there are no passable and uncovered grids in the four directions adjacent to the grid, it is determined that the path falls into a dead zone, and the optimized A* algorithm is selected to escape from the dead zone. The escape target grid is determined by the shortest Euclidean distance. Since there may be a grid in the escape path that is not in the dead zone, the escape path planning is stopped at this time, and the grid is replaced as the escape target grid to avoid a lengthy escape path and increase additional energy consumption.
7. The path planning method according to claim 2, characterized in that: The step S70 further includes: S71. Perform path smoothing based on the turning radius parameters of the machine. For machines with a turning radius of zero, construct inscribed arc segments at path turns and U-turns for smoothing. For machines with a turning radius not zero, select a fishtail, bow, or pear shape based on the relationship between the working width and the turning radius. When the turning radius is no greater than half the working width, select a bow shape. When the turning radius is greater than half the working width, select a pear shape or fishtail shape. S72. Based on the selected path smoothing scheme and the planned path, a certain path point and two subsequent path points are selected each time to determine the change in the machine's heading angle. When the path turns or turns around, the center point, tangent point, and arc length of the arc are calculated according to the geometric relationships of the smoothing scheme. 100 points on the arc length are selected as path points after smoothing. If the path remains straight, only that point is used as a path point for the smoothed path, and the remaining two points need to be re-determined. S73. Perform S72 on each path point in the non-smooth path to obtain a smooth path.
8. The path planning method according to claim 2, wherein: The step S80 further includes: S81. Generate a three-dimensional grid of the elevation map based on the map information model constructed in S20, and cover different elevation grids with different colors to reflect the characteristics of each terrain. S82. On the constructed three-dimensional map, read each smoothed path point and its corresponding elevation information in sequence, connect them with straight lines, and display the planned smoothed path on the map.