Orchard picking robot path planning algorithm

Abstract

An orchard picking robot path planning algorithm comprises the following steps: (1) constructing a two-dimensional grid map according to an orchard top view or a planning map; (2) classifying obstacle areas and customizing a picking sequence; (3) acquiring a peripheral angular point coordinate set of the picking area; (4) determining a connection point set of adjacent regions; (5) performing regional path planning by using an improved A * algorithm and integrating a preliminary path; (6) optimizing the path through an arc fitting smoothing method; and (7) drawing a final path. According to the method, a complex orchard environment can be effectively modeled and analyzed, regional planning and integration are carried out by means of the improved A * algorithm, and complex conditions of numerous obstacles, irregular layout and the like in an orchard can be handled. And an arc fitting smoothing method in path optimization processing further improves the performability and smoothness of the path in a complex environment. The method can adapt to a complex orchard environment, and provides efficient and safe path planning for the picking robot.

Description

Technical Field

[0001] The present invention relates to the technical field of combined optimization of agricultural robots and artificial intelligence, and in particular to a path planning algorithm for an orchard picking robot. Background Art

[0002] In the field of orchard picking robot path planning, existing traditional algorithms mainly include the A* algorithm, the artificial potential field method, the Dijkstra algorithm, etc. Although the Dijkstra algorithm can find the global shortest path, its computational complexity is large, and its planning efficiency is low in complex orchard environments. In addition, the path is not smooth and natural enough, making it difficult to meet the real-time motion planning requirements of the robot. When faced with numerous obstacles such as trees and branches in the orchard and irregular orchard layouts, the traditional A* algorithm, due to the limitations of the heuristic function, plans paths that are excessively tortuous and not smooth enough, resulting in increased energy consumption and poor passability of the robot, affecting the smoothness and efficiency of its picking operations. Due to the limitations of the potential field function design of the artificial potential field method, the robot is prone to falling into local minima in complex orchard environments, making it difficult to find an effective picking path. Summary of the Invention

[0003] In response to the technical problems caused by defects in the above-mentioned traditional algorithms, this technical solution provides a path planning algorithm for an orchard picking robot. It effectively models and analyzes complex orchard environments by constructing a two-dimensional grid map, classifying obstacle areas and customizing the picking order, and obtaining peripheral corner points. It optimizes the algorithm strategy and uses the improved A* algorithm for regional planning and integration. It adopts more reasonable heuristic functions and regional planning, etc., which can handle numerous obstacles and irregular layouts in the orchard, and effectively solves the shortcomings of traditional methods in complex orchard environments. The path optimization adopts the arc fitting smoothing method, which can generate a smooth and efficient picking path, improve the path executability and fluency, and enable the robot to complete the picking task efficiently and safely in complex orchards, thereby improving the operating performance of the orchard picking robot. It can effectively solve the above-mentioned problems.

[0004] The present invention is achieved through the following technical solutions:

[0005] A path planning algorithm for an orchard picking robot includes the following steps:

[0006] Step 1: Determine the orchard boundary and obstacle locations using a bird's-eye view of the orchard or referencing the orchard planning map, and convert them into a two-dimensional raster map.

[0007] Step 2: Divide the obstacle area into picking areas and non-picking areas, and customize the operation sequence of the picking areas according to user needs to meet different picking strategies;

[0008] Step 3: Based on the user-defined operation sequence, determine the outer corner points of each picking area to form multiple corner point coordinate sets. The outer corner points are key points on the picking area boundary, and their coordinates are determined by geometric calculation methods. Specifically, for each picking area, use breadth-first search to find the boundary of the connected obstacle area, and then calculate the outer diagonal coordinates of the four corner points of the boundary to form a set of corner point coordinates of the picking area. This set can fully describe the shape and boundary of the picking area.

[0009] Step 4: Add a starting point and an end point to the beginning and end of the set of corner point coordinates obtained in step 3, where the added starting point is the starting point of the first connection point. Use Euclidean distance to determine the end point of the first connection point, and the area to which the end point belongs is the first area. Then, use the neighbor point method to determine the candidate point for the starting point of the connection point between this area and the next area, and select the end point of the connection point based on the Euclidean distance between the candidate point and each point in the next area. Finally, use this method to obtain the connection points of all adjacent areas to form a set of regional connection points.

[0010] Step 5: Using the improved A* algorithm, perform path planning for each collection area based on the regional connection points in step 4; then sequentially integrate the obtained regionalized paths to construct a preliminary path;

[0011] Step 6: The preliminary path obtained in step 5 is fitted with an arc at the vertical turning point using the arc fitting smoothing method to generate a smooth turning path. The arc fitting smoothing method inserts an arc path at the vertical turning point to replace the original right-angle turn. The radius of the arc can be set according to the turning performance of the robot.

[0012] Step 7: Draw the final path optimized in step 6 onto a two-dimensional grid map to generate an intuitive visualization path.

[0013] Furthermore, the formula for updating the connected region boundary by breadth-first search in step 3 is as follows:

[0014]

[0015] Among them, min_x represents the minimum coordinate of the current connected area on the x-axis; max_x represents the maximum coordinate of the current connected area on the x-axis; min_y represents the minimum coordinate of the current connected area on the y-axis; max_y represents the maximum coordinate of the current connected area on the y-axis; x, y represent the coordinates of the obstacle point currently being processed; min represents the smaller of the two values; max represents the larger of the two values;

[0016] The formula for calculating the outer diagonals of the four corner points of the connected region is as follows:

[0017]

[0018] Among them, A represents the outer diagonal of the lower left corner of the current connected area; B represents the outer diagonal of the upper left corner of the current connected area; C represents the outer diagonal of the upper right corner of the current connected area; D represents the outer diagonal of the lower right corner of the current connected area.

[0019] Furthermore, the specific steps of the neighboring point method in step 4 are as follows: set the end point of a connection point as the reference point, find the outer corner point on the same x-axis or y-axis as the reference point in the area to which the reference point belongs, and use it as the candidate point for the starting point of the next connection point; then use the Euclidean distance as the judgment standard, and use the candidate point with the smallest Euclidean distance as the starting point of the next connection point; the formula for the Euclidean distance is:

[0020]

[0021] Where d represents the Euclidean distance; x g ,y g Represents the x and y values ​​of the target point respectively; x n ,y n Represents the x and y values ​​of the current point respectively.

[0022] Furthermore, the improved A* algorithm described in step 5 is based on the traditional A* algorithm. In order to adapt to the path planning of the orchard picking robot, the heuristic function h(n) is optimized. The heuristic function h(n) takes into account that the robot needs to be close to the picking area in the orchard. The specific formula of the improved heuristic function h(n) is:

[0023] h(n)=α×(|x g -x n |+|y g -y n |)

[0024] Among them, α is the weight coefficient; x g ,y g Represents the x and y values ​​of the target point respectively; x n ,y n Represents the x and y values ​​of the current point respectively.

[0025] Furthermore, the arc fitting smoothing method described in step 6 includes an optimization step for the arc path, wherein the inflection point is determined by the verticality of the vector, and then a better inner arc is generated according to the position of the inflection point and the set arc radius. To avoid collision with obstacles, the outer arc is mirrored with the central axis as the axis, and finally the right-angle inflection point of the preliminary path is replaced by the obtained arc point, so that the connection between the arc path and the preliminary path is smoother, while ensuring the stability and safety of the robot during the turning process; the specific operation method includes: for each right-angle inflection point in the path, the inflection point is first detected, and the presence of a right-angle inflection point is determined by calculating the vectors of adjacent line segments and judging whether they are perpendicular; the vector verticality judgment formula is:

[0026]

[0027] Among them, v1 represents the first vector, which refers to (x i-1 ,y i-1 ) to (x i ,y i ) direction vector; v2 represents the second vector, which refers to (x i ,y i ) to (x i+1 ,y i+1 ) direction vector; point (x i ,y i ) is the current path point; point (x i-1 ,y i-1 ) is the previous path point; point (x i+1 ,y i+1 ) is the next path point; when the dot product v1·v2 of the two vectors is zero, it means that v1 and v2 are perpendicular, that is, there is a right-angle inflection point;

[0028] Then, the radius is set according to the robot's turning performance, and the coordinates of the arc's starting and ending points are determined based on the coordinates of the right-angle inflection point to generate the inner arc point. The central axis is then determined based on the arc's starting and ending points, and the final arc point is obtained by mirroring. Finally, the generated arc point is inserted into the preliminary path, replacing the right-angle inflection point to form a smooth turning path.

[0029] Furthermore, the inner arc generation in step 6 includes two cases: horizontal entry and vertical exit, and vertical entry and horizontal exit.

[0030] The formula for generating the arc point with horizontal entry and vertical exit is:

[0031]

[0032] Among them, (x, y) is the coordinate of the generated arc point; (start_x, start_y) represents the x and y coordinates of the starting point of the arc; (end_x, end_y) represents the x and y coordinates of the end point of the arc; radius represents the radius of the arc; angle represents the angle parameter used when generating the arc, ranging from 0 to π / 2; cos(angle) represents the cosine value of the angle, and sin(angle) represents the sine value of the angle, both of which are used to calculate the position on the arc; sign(v1[0]) represents the sign of the component of vector v1 on the x-axis, and sign(v2[1]) represents the sign of the component of vector v2 on the y-axis, both of which are used to determine the direction;

[0033] The formula for generating the arc point with vertical entry and horizontal exit is:

[0034]

[0035] Among them, (x, y) are the coordinates of the generated arc point; (start_x, start_y) represent the x and y coordinates of the starting point of the arc; (end_x, end_y) represent the x and y coordinates of the end point of the arc; radius represents the radius of the arc; angle represents the angle parameter used when generating the arc, ranging from 0 to π / 2; cos(angle) represents the cosine value of the angle, and sin(angle) represents the sine value of the angle, both of which are used to calculate the position on the arc; sign(v2[0]) represents the sign of the component of vector v2 on the x-axis, and sign(v1[1]) represents the sign of the component of vector v1 on the y-axis, both of which are used to determine the direction.

[0036] Furthermore, in step 6, the outer arc is mirrored with the central axis as the axis. First, the axis direction needs to be determined. The axis direction vector calculation formula is:

[0037]

[0038] Among them, axis_dx and axis_dy represent the components of the axis direction vector on the x-axis and y-axis respectively; (end_x, end_y) represents the coordinates of the end point of the arc; (start x ,start_y) represents the coordinates of the starting point of the arc;

[0039] Then determine the central axis unit vector. The calculation formula of the central axis unit vector is:

[0040]

[0041] Among them, length represents the modulus of the central axis direction vector, which is used to normalize the direction vector; unit_dx represents the component of the unit vector in the central axis direction on the x-axis; unit_dy represents the component of the unit vector in the central axis direction on the y-axis;

[0042] Next, determine the relative coordinates of the outer arc. The calculation formula for the relative coordinates of the outer arc is:

[0043]

[0044] Among them, rel_x represents the offset of the generated arc point relative to the starting point on the x-axis; rel_y represents the offset of the generated arc point relative to the starting point on the y-axis;

[0045] Then determine the vector projection. The calculation formula for vector projection is:

[0046]

[0047] Among them, dot_product represents the dot product of the relative coordinate vector and the unit vector of the central axis, which represents the projection length of the relative coordinate vector in the direction of the central axis; proj_x represents the component of the projection vector on the x-axis; proj_y represents the component of the projection vector on the y-axis;

[0048] Finally, the mirror flip calculation formula and absolute coordinate conversion formula are:

[0049]

[0050] Among them, f_rel_x represents the component of the relative coordinate on the x-axis after the mirror flip; f_rel_y represents the component of the relative coordinate on the y-axis after the mirror flip;

[0051]

[0052] Among them, x ' Indicates the absolute x coordinate of the arc point after mirror flipping; y ' Indicates the absolute y-coordinate of the arc point after mirror flipping.

[0053] Furthermore, the visualized path described in step 7 can use different colors or line styles to represent paths and areas, such as feasible paths, obstacle areas, and harvesting areas, so that users can intuitively understand the path planning results. The method for drawing the visualized path includes the following steps: first, constructing a grid map to divide the orchard environment into grids and marking the locations of harvesting areas and non-harvesting areas; then, marking the explored locations in blue; and finally, drawing the resulting planned path with a dotted line.

[0054] Furthermore, the geometric conversion described in step 1 is to convert the actual size of the orchard into grid coordinates in the two-dimensional grid map according to a preset scale factor. The scale factor can be adjusted according to the size of the orchard and the resolution of the map. Each grid is used to represent a specific area in the orchard, and its status is passable or obstacle.

[0055] Furthermore, in step 2, user requirements may be input through a human-computer interaction interface, including but not limited to a priority picking order determined by factors such as tree species and geographical location.

[0056] Beneficial effects

[0057] The path planning algorithm for an orchard picking robot proposed in this invention has the following advantages compared with the prior art:

[0058] This invention accurately models and analyzes complex orchard environments by constructing a two-dimensional grid map, classifying obstacle areas, customizing the picking order, and obtaining peripheral corner points. This method also optimizes the limitations of the traditional A* algorithm in orchard environments and employs an improved A* algorithm for regional path planning and integration. The core of the improved A* algorithm lies in the reconstruction of the heuristic function h(n). By introducing a weighting coefficient α that considers the distance between the robot and the target picking area, the planning process is more tailored to the actual needs of orchard picking tasks. The improved heuristic function h(n) can more accurately guide the robot to the target picking point, effectively reducing redundant calculations in path planning and improving path planning efficiency. Compared to the traditional A* algorithm, this improvement significantly shortens path planning time and reduces the delay in the robot finding an effective path in complex orchards, thereby improving overall operational efficiency. Furthermore, the regional planning and integration strategy enables the robot to better adapt to the numerous obstacles and irregular layout of the orchard, avoiding the problems of excessive path tortuosity and local minima that plague traditional algorithms in complex environments. Path optimization utilizes arc fitting and smoothing, further improving path execution and smoothness. This reduces the robot's acceleration and deceleration frequency during turns and obstacle avoidance, lowering energy consumption and improving motion stability. Furthermore, the generated visualized path not only allows users to intuitively understand the planning results but also serves as a basis for subsequent optimization, enhancing system transparency and controllability.

[0059] The path planning method of the present invention not only improves the operating performance of the orchard picking robot in complex environments, but also expands its scope of application in irregular orchard layouts, provides more reliable and efficient technical support for agricultural automation, and promotes the development of intelligent agricultural machinery technology. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 It is a schematic diagram of the overall process of the present invention.

[0061] Figure 2 This is a schematic diagram of the division of picking and non-picking areas in the present invention.

[0062] Figure 3 This is a local graph of path optimization based on the arc fitting smoothing method in the present invention.

[0063] Figure 4 This is the orchard path planning effect diagram in the present invention. DETAILED DESCRIPTION

[0064] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. The described embodiments are only part of the embodiments of the present invention, rather than all the embodiments. Under the premise of not departing from the design concept of the present invention, various modifications and improvements made by ordinary persons in this field to the technical solutions of the present invention should fall within the scope of protection of the present invention.

[0065] Example 1:

[0066] like Figure 1 As shown in FIG, a path planning algorithm for an orchard picking robot includes the following steps:

[0067] Step 1: Using a bird's-eye view of the orchard or a reference orchard plan, determine the orchard boundaries and the locations of any obstacles, and then convert them to a two-dimensional grid map. This conversion involves converting the orchard's actual dimensions into grid coordinates within the two-dimensional grid map using a preset scale factor. The scale factor can be adjusted based on the orchard's size and the map's resolution. Each grid cell represents a specific area in the orchard, indicating whether it is accessible or an obstacle.

[0068] Step 2: If Figure 2 As shown, the obstacle area is divided into picking and non-picking areas, with red areas representing picking areas and orange areas representing non-picking areas. The order of operations in the picking areas can then be customized based on user needs to meet different picking strategies. These user needs can be input through the human-computer interaction interface, including but not limited to priority picking orders based on factors such as tree species and geographic location.

[0069] Step 3: According to the user-defined operation sequence, determine the outer corner points of each picking area to form multiple corner point coordinate sets.

[0070] The outer corner points refer to key points on the boundary of the picking area, and their coordinates are determined by geometric calculation methods; specifically: for each picking area, the boundary of the connected obstacle area is found through breadth-first search, and then the outer diagonal coordinates of the four corner points of the boundary are calculated to form a set of corner point coordinates of the picking area, which can fully describe the shape and boundary of the picking area.

[0071] The formula for updating the boundaries of connected regions using breadth-first search is as follows:

[0072]

[0073] Among them, min_x represents the minimum coordinate of the current connected area on the x-axis; max_x represents the maximum coordinate of the current connected area on the x-axis; min_y represents the minimum coordinate of the current connected area on the y-axis; max_y represents the maximum coordinate of the current connected area on the y-axis; x, y represent the coordinates of the obstacle point currently being processed; min represents the smaller of the two values; max represents the larger of the two values;

[0074] The formula for calculating the outer diagonals of the four corner points of the connected region is as follows:

[0075]

[0076] Among them, A represents the outer diagonal of the lower left corner of the current connected area; B represents the outer diagonal of the upper left corner of the current connected area; C represents the outer diagonal of the upper right corner of the current connected area; D represents the outer diagonal of the lower right corner of the current connected area.

[0077] Step 4: Add a starting point and an end point to the beginning and end of the corner point coordinate set obtained in step 3, where the added starting point is the starting point of the first connection point. The end point of the first connection point is determined using the Euclidean distance, and the area to which the end point belongs is the first area. Subsequently, the neighboring point method is used to determine the candidate point for the starting point of the connection point between this area and the next area, and the end point of the connection point is selected based on the Euclidean distance between the candidate point and each point in the next area. Finally, this method is used to obtain the connection points of all adjacent areas to form a set of regional connection points.

[0078] The specific steps of the neighbor point method are as follows: set the end point of the previous connection point as the reference point, find the outer corner point on the same x-axis or y-axis as the reference point in the area to which the reference point belongs, and use it as the candidate point for the starting point of the next connection point; then use the Euclidean distance as the judgment standard, and use the candidate point with the smallest Euclidean distance as the starting point of the next connection point; the formula for the Euclidean distance is:

[0079]

[0080] Where d represents the Euclidean distance; x g ,y g Represents the x and y values ​​of the target point respectively; x n ,y n Represents the x and y values ​​of the current point respectively.

[0081] Step 5: Using the improved A* algorithm, perform path planning for each collection area based on the regional connection points in step 4; then sequentially integrate the obtained regionalized paths to construct a preliminary path.

[0082] The improved A* algorithm is based on the traditional A* algorithm. In order to adapt to the path planning of the orchard picking robot, the heuristic function h(n) is optimized. The heuristic function h(n) takes into account that the robot needs to be close to the picking area in the orchard. The specific formula of the improved heuristic function h(n) is:

[0083] h(n)=α×(|x g -x n |+|y g -y n |)

[0084] Among them, α is the weight coefficient; x g ,y g Represents the x and y values ​​of the target point respectively; x n ,y n Represents the x and y values ​​of the current point respectively.

[0085] Step 6: For the preliminary path obtained in step 5, use the arc fitting smoothing method to fit the vertical inflection point with an arc to generate a smooth turning path; Figure 3 As shown in the figure, the arc fitting smoothing method replaces the original right-angle turn by inserting a circular arc path at the vertical inflection point. The radius of the arc can be set according to the turning performance of the robot.

[0086] The arc fitting smoothing method includes the steps of optimizing the arc path, judging the inflection point through vector perpendicularity, and then generating a better inner arc according to the inflection point position and the set arc radius. In order to avoid collision with obstacles, the outer arc is mirrored with the central axis as the axis, and finally the obtained arc point replaces the right-angle inflection point of the preliminary path, so that the connection between the arc path and the preliminary path is smoother, while ensuring the stability and safety of the robot during the turning process. Specifically, for each right-angle inflection point in the path, the inflection point is first detected, and the existence of a right-angle inflection point is determined by calculating the vectors of adjacent line segments and judging whether they are perpendicular; then the radius is set according to the turning performance of the robot, and the coordinates of the arc start and end points are determined in combination with the coordinates of the right-angle inflection point, and then the inner arc point is generated; then the central axis is determined by the arc start and end points, and then the final arc point is obtained by mirroring; finally, the arc point is inserted, and the generated arc point is inserted into the preliminary path to replace the right-angle inflection point to form a smooth turning path. The vector perpendicularity judgment formula is:

[0087]

[0088] Among them, v1 represents the first vector, which refers to (x i-1 ,y i-1 ) to (x i ,y i ) direction vector; v2 represents the second vector, which refers to (x i ,y i ) to (xi+1 ,y i+1 ) direction vector; point (x i ,y i ) is the current path point; point (x i-1 ,y i-1 ) is the previous path point; point (x i+1 ,y i+1 ) is the next path point; when the dot product v1·v2 of the two vectors is zero, it means that v1 and v2 are perpendicular, that is, there is a right-angle inflection point.

[0089] Then, the radius is set according to the robot's turning performance, and the coordinates of the arc's starting and ending points are determined based on the coordinates of the right-angle inflection point to generate the inner arc point. The central axis is then determined based on the arc's starting and ending points, and the final arc point is obtained by mirroring. Finally, the generated arc point is inserted into the preliminary path, replacing the right-angle inflection point to form a smooth turning path.

[0090] The inner arc generation includes two cases: horizontal entry and vertical exit and vertical entry and horizontal exit.

[0091] The formula for generating the arc point with horizontal entry and vertical exit is:

[0092]

[0093] Among them, (x, y) are the coordinates of the generated arc point; (start_x, start_y) represent the x and y coordinates of the starting point of the arc; (end_x, end_y) represent the x and y coordinates of the end point of the arc; radius represents the radius of the arc; angle represents the angle parameter used when generating the arc, ranging from 0 to π / 2; cos(angle) represents the cosine value of the angle, and sin(angle) represents the sine value of the angle, both of which are used to calculate the position on the arc; sign(v1[0]) represents the sign of the component of vector v1 on the x-axis, and sign(v2[1]) represents the sign of the component of vector v2 on the y-axis, both of which are used to determine the direction.

[0094] The formula for generating the arc point with vertical entry and horizontal exit is:

[0095]

[0096] Among them, (x, y) are the coordinates of the generated arc point; (start_x, start_y) represent the x and y coordinates of the starting point of the arc; (end_x, end_y) represent the x and y coordinates of the end point of the arc; radius represents the radius of the arc; angle represents the angle parameter used when generating the arc, ranging from 0 to π / 2; cos(angle) represents the cosine value of the angle, and sin(angle) represents the sine value of the angle, both of which are used to calculate the position on the arc; sign(v2[0]) represents the sign of the component of vector v2 on the x-axis, and sign(v1[1]) represents the sign of the component of vector v1 on the y-axis, both of which are used to determine the direction.

[0097] To mirror the outer arc with the central axis as the axis, the axis direction must first be determined. The axis direction vector calculation formula is:

[0098]

[0099] Among them, axis_dx and axis_dy represent the components of the axis direction vector on the x-axis and y-axis respectively; (end_x, end_y) represents the coordinates of the end point of the arc; (start x ,start_y) represents the coordinates of the starting point of the arc.

[0100] Then determine the central axis unit vector. The calculation formula of the central axis unit vector is:

[0101]

[0102] Among them, length represents the modulus of the central axis vector, which is used to normalize the direction vector; unit_dx represents the component of the unit vector in the central axis direction on the x-axis; unit_dy represents the component of the unit vector in the central axis direction on the y-axis.

[0103] Next, determine the relative coordinates of the outer arc. The calculation formula for the relative coordinates of the outer arc is:

[0104]

[0105] Among them, rel_x represents the offset of the generated arc point relative to the starting point on the x-axis; rel_y represents the offset of the generated arc point relative to the starting point on the y-axis.

[0106] Then determine the vector projection. The calculation formula for vector projection is:

[0107]

[0108] Among them, dot_product represents the dot product of the relative coordinate vector and the unit vector of the central axis, which represents the projection length of the relative coordinate vector in the direction of the central axis; proj_x represents the component of the projection vector on the x-axis; proj_y represents the component of the projection vector on the y-axis.

[0109] Finally, the mirror flip calculation formula and absolute coordinate conversion formula are:

[0110]

[0111] Among them, f_rel_x represents the component of the relative coordinate on the x-axis after the mirror flip; f_rel_y represents the component of the relative coordinate on the y-axis after the mirror flip.

[0112]

[0113] Among them, x ' Indicates the absolute x coordinate of the arc point after mirror flipping; y ' Indicates the absolute y-coordinate of the arc point after mirror flipping.

[0114] Step 7: Draw the final path optimized in step 6 onto a two-dimensional grid map to generate an intuitive visualization path. The visualization path can represent paths and areas using different colors or line styles, such as Figure 4 The blue dotted line represents the feasible path, the orange part represents the obstacle area, and the red part represents the picking area, so that users can intuitively understand the path planning results.

[0115] The method for drawing a visual path includes the following steps: first, constructing a grid map, dividing the orchard environment into grids, and marking the locations of collection areas and non-collection areas; then marking the exploration locations in blue; and finally drawing the resulting planned path with dotted lines.

[0116] The above embodiments are intended only to illustrate the technical concepts and features of the present invention. Their purpose is to enable those skilled in the art to understand the contents of the present invention and implement them accordingly. They are not intended to limit the scope of protection of the present invention. Any equivalent changes or modifications made in accordance with the spirit of the present invention shall be covered by the present invention.

Claims

1. A path planning algorithm for an orchard picking robot, characterized by: Including steps: Step 1: Determine the orchard boundary and obstacle locations using a bird's-eye view of the orchard or referencing the orchard planning map, and convert them into a two-dimensional raster map. Step 2: Divide the obstacle area into picking areas and non-picking areas, and customize the operation sequence of the picking areas according to user needs to meet different picking strategies; Step 3: Based on the user-defined operation sequence, determine the outer corner points of each picking area to form multiple corner point coordinate sets. The outer corner points are key points on the picking area boundary, and their coordinates are determined by geometric calculation methods. Specifically, for each picking area, use breadth-first search to find the boundary of the connected obstacle area, and then calculate the outer diagonal coordinates of the four corner points of the boundary to form a set of corner point coordinates of the picking area. This set can fully describe the shape and boundary of the picking area. Step 4: Add a starting point and an end point to the beginning and end of the set of corner point coordinates obtained in step 3. The added starting point is the starting point of the first connection point. The end point of the first connection point is determined using Euclidean distance. The area to which the end point belongs is the first area. Then, the neighbor point method is used to determine the candidate points for the starting point of the connection point between this region and the next region, and the end point of the connection point is selected based on the Euclidean distance between the candidate point and each point in the next region; finally, the connection points of all adjacent regions are obtained using this method to form a set of regional connection points; Step 5: Using the improved A* algorithm, perform path planning for each collection area based on the regional connection points in step 4; then sequentially integrate the obtained regionalized paths to construct a preliminary path; Step 6: For the preliminary path obtained in step 5, use the arc fitting smoothing method to fit an arc at the vertical inflection point to generate a smooth turning path; The arc fitting smoothing method replaces the original right-angle turn by inserting an arc path at the vertical inflection point. The radius of the arc can be set according to the turning performance of the robot. Step 7: Draw the final path optimized in step 6 onto a two-dimensional grid map to generate an intuitive visualization path.

2. The path planning algorithm for an orchard picking robot according to claim 1, characterized in that: The formula for updating the connected region boundary by breadth-first search in step 3 is as follows: Among them, min_x represents the minimum coordinate of the current connected area on the x-axis; max_x represents the maximum coordinate of the current connected area on the x-axis; min_y represents the minimum coordinate of the current connected area on the y-axis; max_y represents the maximum coordinate of the current connected area on the y-axis; x, y represent the coordinates of the obstacle point currently being processed; min represents the smaller of the two values; max represents the larger of the two values; The formula for calculating the outer diagonals of the four corner points of the connected region is as follows: Among them, A represents the outer diagonal of the lower left corner of the current connected area; B represents the outer diagonal of the upper left corner of the current connected area; C represents the outer diagonal of the upper right corner of the current connected area; D represents the outer diagonal of the lower right corner of the current connected area.

3. The path planning algorithm for an orchard picking robot according to claim 1, characterized in that: The specific steps of the neighboring point method in step 4 are as follows: set the end point of a connection point as the reference point, find the outer corner point on the same x-axis or y-axis as the reference point in the area to which the reference point belongs, and use it as the candidate point for the starting point of the next connection point; then use the Euclidean distance as the judgment standard, and use the candidate point with the smallest Euclidean distance as the starting point of the next connection point; the formula for the Euclidean distance is: Where d represents the Euclidean distance; x g ,y g Represents the x and y values ​​of the target point respectively; x n ,y n Represents the x and y values ​​of the current point respectively.

4. The path planning algorithm for an orchard picking robot according to claim 1, characterized in that: The improved A* algorithm described in step 5 is based on the traditional A* algorithm. In order to adapt to the path planning of the orchard picking robot, the heuristic function h(n) is optimized. The heuristic function h(n) takes into account that the robot needs to be close to the picking area in the orchard. The specific formula of the improved heuristic function h(n) is: h(n)=α×(|x g -x n |+|y g -y n |) Among them, α is the weight coefficient; x g ,y g Represents the x and y values ​​of the target point respectively; x n ,y n Represents the x and y values ​​of the current point respectively.

5. The path planning algorithm for an orchard picking robot according to claim 1, characterized in that: The arc fitting smoothing method described in step 6 includes the steps of optimizing the arc path, determining the inflection point by vector perpendicularity, then generating a better inner arc according to the inflection point position and the set arc radius, then mirroring the outer arc with the central axis as the axis, and finally replacing the right-angle inflection point of the preliminary path with the obtained arc point; the specific operation method includes: for each right-angle inflection point in the path, first detect the inflection point, and determine whether there is a right-angle inflection point by calculating the vectors of adjacent line segments and judging whether they are perpendicular; the vector perpendicularity judgment formula is: Among them, v1 represents the first vector, which refers to (x i-1 ,y i-1 ) to (x i ,y i ) direction vector; v2 represents the second vector, which refers to (x i ,y i ) to (x i+1 ,y i+1 ) direction vector; point (x i ,y i ) is the current path point; point (x i-1 ,y i-1 ) is the previous path point; point (x i+1 ,y i+1 ) is the next path point; when the dot product v1·v2 of the two vectors is zero, it means that v1 and v2 are perpendicular, that is, there is a right-angle inflection point; Then, the radius is set according to the robot's turning performance, and the coordinates of the arc's starting and ending points are determined based on the coordinates of the right-angle inflection point to generate the inner arc point. The central axis is then determined based on the arc's starting and ending points, and the final arc point is obtained by mirroring. Finally, the generated arc point is inserted into the preliminary path, replacing the right-angle inflection point to form a smooth turning path.

6. The path planning algorithm for an orchard picking robot according to claim 5, characterized in that: The inner arc generation in step 6 includes two cases: horizontal entry and vertical exit, and vertical entry and horizontal exit; The formula for generating the arc point with horizontal entry and vertical exit is: Among them, (x, y) is the coordinate of the generated arc point; (start_x, start_y) represents the x and y coordinates of the starting point of the arc; (end_x, end_y) represents the x and y coordinates of the end point of the arc; radius represents the radius of the arc; angle represents the angle parameter used when generating the arc, ranging from 0 to π / 2; cos(angle) represents the cosine value of the angle, and sin(angle) represents the sine value of the angle, both of which are used to calculate the position on the arc; sign(v1[0]) represents the sign of the component of vector v1 on the x-axis, and sign(v2[1]) represents the sign of the component of vector v2 on the y-axis, both of which are used to determine the direction; The formula for generating the arc point with vertical entry and horizontal exit is: Among them, (x, y) are the coordinates of the generated arc point; (start_x, start_y) represent the x and y coordinates of the starting point of the arc; (end_x, end_y) represent the x and y coordinates of the end point of the arc; radius represents the radius of the arc; angle represents the angle parameter used when generating the arc, ranging from 0 to π / 2; cos(angle) represents the cosine value of the angle, and sin(angle) represents the sine value of the angle, both of which are used to calculate the position on the arc; sign(v2[0]) represents the sign of the component of vector v2 on the x-axis, and sign(v1[1]) represents the sign of the component of vector v1 on the y-axis, both of which are used to determine the direction.

7. The path planning algorithm for an orchard picking robot according to claim 5, characterized in that: In step 6, to mirror the outer arc with the central axis as the axis, you first need to determine the axis direction. The axis direction vector calculation formula is: Among them, axis_dx and axis_dy represent the components of the axis direction vector on the x-axis and y-axis respectively; (end_x, end_y) represents the coordinates of the end point of the arc; (start x ,start_y) represents the coordinates of the starting point of the arc; Then determine the central axis unit vector. The calculation formula of the central axis unit vector is: Among them, length represents the modulus of the central axis direction vector, which is used to normalize the direction vector; unit_dx represents the component of the unit vector in the central axis direction on the x-axis; unit_dy represents the component of the unit vector in the central axis direction on the y-axis; Next, determine the relative coordinates of the outer arc. The calculation formula for the relative coordinates of the outer arc is: Among them, rel_x represents the offset of the generated arc point relative to the starting point on the x-axis; rel_y represents the offset of the generated arc point relative to the starting point on the y-axis; Then determine the vector projection. The calculation formula for vector projection is: Among them, dot_product represents the dot product of the relative coordinate vector and the unit vector of the central axis, which represents the projection length of the relative coordinate vector in the direction of the central axis; proj_x represents the component of the projection vector on the x-axis; proj_y represents the component of the projection vector on the y-axis; Finally, the mirror flip calculation formula and absolute coordinate conversion formula are: Among them, f_rel_x represents the component of the relative coordinate on the x-axis after the mirror flip; f_rel_y represents the component of the relative coordinate on the y-axis after the mirror flip; Where x' represents the absolute x-coordinate of the arc point after mirror flipping; y' represents the absolute y-coordinate of the arc point after mirror flipping.

8. The path planning algorithm for an orchard picking robot according to claim 1, characterized in that: The visualization path described in step 7 can represent paths and areas using different colors or line styles, wherein the method for drawing the visualization path includes the following steps: first, constructing a grid map, dividing the orchard environment into grids, and marking the locations of the collection area and the non-collection area; then marking the exploration location in blue; and finally drawing the resulting planned path with a dotted line.

9. The path planning algorithm for an orchard picking robot according to claim 1, characterized in that: The geometric conversion described in step 1 is to convert the actual size of the orchard into grid coordinates in the two-dimensional grid map according to a preset scale factor. The scale factor can be adjusted according to the size of the orchard and the resolution of the map. Each grid is used to represent a specific area in the orchard, and its status is passable or obstacle.

10. The path planning algorithm for an orchard picking robot according to claim 1, characterized in that: Step 2: User requirements may be input through the human-computer interaction interface, including but not limited to a priority picking order determined by factors such as tree species and geographical location.

Images