Orchard shortest path planning method based on laser radar SLAM map

Abstract

The invention discloses an orchard shortest path planning method based on a laser radar SLAM map. The method is used for autonomous navigation of an orchard operation robot. Firstly, orchard three-dimensional point cloud data are collected by using a laser radar SLAM technology, and a high-precision map is constructed. Then carrying out downsampling and noise removal on the point cloud data, extracting ground point cloud and calculating gradient information; thirdly, segmenting the point cloud through a random forest model, distinguishing passing and non-passing areas, extracting the center coordinates of the fruit tree through an improved DBSCAN algorithm, and judging the row direction of the fruit tree through kernel density estimation; then, a cost map containing a gradient cost layer is constructed, and the path length, gradient information and obstacle distribution are comprehensively considered; an improved A * algorithm is adopted to introduce cost map information, a 16 adjacent mode is used to optimize path smoothness, a Bezier curve is used to further carry out local smoothing processing on the path, and a shortest path is generated. The method integrates multiple technologies, is suitable for a complex orchard environment, and has high robustness and applicability.

Description

Technical Field

[0001] The present invention relates to the technical field of agricultural robot navigation, in particular to an orchard shortest path planning method based on a lidar SLAM map. Background Art

[0002] In modern agricultural production, the mechanization and automation of orchard operations are the key to improving production efficiency and reducing labor costs. As a major fruit-producing country in the world, China ranks among the top in terms of orchard planting area and output. However, the mechanization level of orchard operations is still relatively low, mainly relying on manual operation. This traditional operation method not only has a high labor intensity but also low efficiency, making it difficult to meet the needs of modern agricultural development. Therefore, developing an automated navigation technology that can adapt to complex orchard environments is of great practical significance for improving the efficiency and quality of orchard operations.

[0003] In recent years, with the rapid development of navigation technologies, technologies such as satellite positioning systems (GNSS), lidar (LiDAR), and machine vision have gradually been applied to orchard robot navigation. However, these technologies all face different degrees of challenges in the orchard environment. Due to the influence of tree occlusion and complex terrain in the orchard, the satellite positioning system is prone to signal loss or drift, resulting in insufficient positioning accuracy and making it difficult to meet the requirements of robot autonomous navigation. Although machine vision technology has advantages in image recognition, in the orchard environment, the interference of light changes, shadows, and complex backgrounds makes it difficult to maintain stable performance, especially at night or under extreme weather conditions, and its limitations are more obvious.

[0004] In contrast, lidar (LiDAR) technology has gradually become a popular choice for orchard robot navigation due to its high precision, long detection distance, and insensitivity to lighting conditions. The lidar SLAM (Simultaneous Localization and Mapping) technology can construct a high-precision map in real time and determine the robot's position in a complex environment by fusing lidar data and robot motion information, providing a solid foundation for robot autonomous navigation.

[0005] In terms of path planning, the shortest path planning is an important research direction in orchard robot navigation. Traditional path planning algorithms, such as the A* algorithm, although they can generate a path from the starting point to the end point, in the complex orchard environment, these algorithms often do not fully consider factors such as terrain slope, obstacle distribution, and fruit tree row direction, resulting in the generated path not being smooth enough and even possibly crossing high-slope areas or approaching obstacles, thus increasing the operation risk.

[0006] In summary, the existing orchard navigation technology has obvious deficiencies in terms of adaptability in complex environments and the need for global path planning capabilities. Therefore, developing an orchard shortest path planning method based on lidar SLAM maps, which can comprehensively consider terrain information, fruit tree recognition, and the globality and safety of path planning, has important practical significance and broad application prospects for promoting orchard mechanization and automation operations. Summary of the Invention

[0007] To solve the problems existing in the existing orchard navigation path planning, such as limited local vision, difficulty in adapting to complex terrains, and insufficient path smoothness, the present invention provides an orchard shortest path planning method based on lidar SLAM maps. Lidar SLAM technology, with its advantages of high precision, strong anti-interference ability, and the ability to construct three-dimensional maps in real time, shows broad application prospects in orchard environments. By performing path planning on the lidar SLAM map, not only can the orchard terrain and fruit tree distribution be accurately perceived, but also the shortest path that avoids high-slope areas, stays away from obstacles, and is smooth and efficient can be generated, thus significantly improving the autonomous navigation ability of orchard robots in complex environments.

[0008] The purpose of the present invention is achieved through the following technical solutions: An orchard shortest path planning method based on lidar SLAM maps, the method comprising the following steps:

[0009] An orchard shortest path planning method based on lidar SLAM maps, the method comprising the following steps:

[0010] S01. Construct an orchard three-dimensional point cloud map: Adopt the LIO-SAM (Lidar-Inertial Odometry and Simultaneous Localization and Mapping) lidar SLAM construction technology to achieve high-precision orchard three-dimensional map construction by fusing the point cloud data of the lidar with the acceleration and angular velocity data of the inertial measurement unit (IMU);

[0011] S02. Point cloud downsampling: Perform downsampling processing on the three-dimensional point cloud map to remove noise points and outliers and reduce the density of the point cloud data; The downsampling adopts a method based on a voxel grid, divides the point cloud space into multiple small voxel grids, and calculates the average position of the points within each grid to represent the point cloud of the voxel;

[0012] S03. Point cloud height screening and slope information acquisition: Perform height screening on the downsampled point cloud data, extract the ground point cloud, and calculate the average slope angle within each grid to obtain slope information. The height screening uses the cloth simulation filtering algorithm (CSF) to separate the ground point cloud from the non-ground point cloud, and divides the separated orchard point cloud and ground point cloud into grids, screening out the orchard point cloud within the height range of the operating vehicle above the ground point. For the ground point cloud within each grid, calculate the horizontal distance and height difference between it and the lowest point, and use the formula to calculate the slope value to obtain the average slope angle of each grid. This slope calculation method will be used to create the subsequent slope cost map.

[0013] S04. Fruit tree point cloud segmentation: Segment the point cloud data after height screening based on an improved random forest model to distinguish the passage area and the non-passage area. In the model training stage of the improved random forest model, by evaluating the importance of each feature and weighting it, the model can focus on key features. At the same time, to address the problem of uneven proportions of different regions in the point cloud data, a class weight adjustment technique is adopted to give higher weights to minority class samples. In addition, the key parameters of the random forest are optimized through cross-validation and grid search to further improve the generalization ability and accuracy of the model.

[0014] S06. Fruit tree center point clustering: Cluster the segmented fruit tree point cloud to extract the center point coordinates of the fruit trees. An improved DBSCAN algorithm is used. First, the clustering parameters are dynamically adjusted using the local point cloud density to adapt to point cloud distributions with different densities. Second, by combining the joint criterion of local density and deviation, noise points are dynamically identified and filtered out. Finally, a density-weighted centroid calculation method is adopted, where different weights are assigned to each point according to the local density of the point cloud, making the contribution of the high-density area to the centroid greater.

[0015] S07. Fruit tree row direction discrimination: Calculate the direction of the tree row through kernel density estimation technology. An estimation method for the fruit tree row direction based on the angles of neighboring points is adopted. First, for each tree point within the field of view, the most neighboring points that meet the conditions are screened out, then each tree point is connected to its most neighboring point, the angle between the connection line and the x-axis is calculated, and the angle is normalized to the interval (-90°, 90°). Further, kernel density estimation is performed on the directions of the most neighboring points of all tree points, and the bandwidth parameter of the kernel density estimation is dynamically calculated to adapt to point cloud distributions with different densities. Finally, the estimated direction of the tree row is determined by calculating the angle with the maximum density of the kernel density estimation.

[0016] S07. Shortest Path Planning Based on Cost Map: Construct a cost map including a slope cost layer, and combine with an improved A* algorithm to generate the shortest path from the starting point to the ending point; the cost map consists of a static obstacle layer, a slope cost layer, and an inflation layer. The improved A* algorithm adds the cost information of the cost map to the cost function, thereby effectively guiding the path to avoid high-slope areas and obstacles. In addition, the 16-adjacent method is used to replace the traditional 8-adjacent method and 4-adjacent method to increase the flexibility of the path, and an additional value is added to the heuristic function to preferentially select nodes with less change in the path direction; finally, local smoothing processing is performed on the generated path, and quadratic Bezier curves are used for smooth interpolation of the turning parts to obtain a smooth and efficient shortest path.

[0017] Further, in step S03:

[0018] (3.1) The method for obtaining the height of the operation vehicle is as follows: The lowest working height of the operation vehicle is measured in advance and this height value is input into the path planning system as a fixed parameter; this height value can be obtained through vehicle design parameters or on-site measurement to ensure its suitability for the orchard operation environment;

[0019] (3.2) The calculation formula is where (x min , y min , z min ) is the point with the minimum z coordinate among all ground point clouds within the current grid.

[0020] Further, in step S07:

[0021] (7.1) When constructing the static obstacle layer of the cost map, it is necessary to accurately identify and label the positions of the fruit tree point clouds. In addition, in order to form the organizational form of tree rows, it is necessary to fit the fruit tree row line segments by the least squares method and screen out the newly fitted line segments whose distances from the selected line segments are greater than the set threshold to ensure the accuracy and representativeness of the fruit tree row line segments, and then convert the geometric features of the fruit tree rows into line segment forms and incorporate them into the obstacle classification system;

[0022] (7.2) When constructing the slope cost layer of the cost map, based on the ground slope information obtained by the cloth filtering algorithm, different cost values are assigned according to the slope size. The greater the slope, the higher the cost, thereby guiding the path to avoid steep slope areas;

[0023] (7.3) When constructing the inflation layer of the cost map, inflation processing is performed on the obstacles to set a safety buffer zone around the obstacles to prevent the robot from getting too close to the obstacles;

[0024] (7.4) Adopt the 16 - adjacency method to replace the traditional 4 - adjacency or 8 - adjacency search method, optimize the smoothness of the path, and at the same time, when planning the path, consider the relationship between nodes and avoid the vertices with threatening grids.

[0025] (7.5) In the heuristic function, when multiple nodes have the same f value, judge the degree of path direction change by calculating the cross - product of the vector from the initial node to the target node and the vector from the current node to the target node, so as to reduce unnecessary turns in the path.

[0026] (7.6) Perform local smoothing on the generated path. Select the two mid - points formed by the turning point and its two adjacent path points before and after as the smoothing points, perform smooth interpolation on the turning part through the quadratic Bezier curve, and splice the smooth line segment with the unmodified original path segment to obtain a smooth and efficient shortest path.

[0027] The beneficial effects of the present invention are as follows: The method of the present invention realizes the efficient generation of the global shortest path in the orchard. The method of the present invention can adapt to complex environmental conditions such as different orchard terrains, different fruit tree planting densities, and different tree row spacings. Compared with the GNSS - based method, the method of the present invention can operate stably in complex environments such as tree occlusion and generate high - precision navigation paths. Compared with the local - sensor - based method, through global map construction and path planning, the method of the present invention can generate a smoother and safer path, avoiding the robot from crossing high - slope areas or approaching obstacles during operation. The generated path not only helps to improve the operation efficiency and safety of the orchard robot, but also provides reliable technical support for orchard automation operations. The method of the present invention is applicable to various orchard environments and has high popularization and application value. Description of the Drawings

[0028] Figure 1 It is a schematic flow chart of a method for planning the shortest path in an orchard based on a lidar SLAM map in an embodiment of the present invention.

[0029] Figure 2 It is a real picture of an example orchard in an embodiment of the present invention.

[0030] Figure 3 It is a lidar SLAM picture constructed for the example orchard in an embodiment of the present invention.

[0031] Figure 4 It is a schematic diagram of the principle of the point cloud height screening and slope information calculation process in an embodiment of the present invention.

[0032] Figure 5 It is a segmentation effect picture of an example test orchard obtained by using an improved random forest model in an embodiment of the present invention.

[0033] Figure 6This is the extraction effect diagram of the center point of fruit trees in the example of the present invention.

[0034] Figure 7(a) is a schematic diagram of the discrimination process of the fruit tree row direction in the example of the present invention: the obtained center point of the fruit tree;

[0035] Figure 7(b) is a schematic diagram of the discrimination process of the fruit tree row direction in the example of the present invention: showing the process of selecting the nearest neighbor point for each tree point in the field of view and connecting them with line segments;

[0036] Figure 7(c) is a schematic diagram of the discrimination process of the fruit tree row direction in the example of the present invention: showing the process of normalizing the angles of these target line segments to (-90°, 90°);

[0037] Figure 7(d) is a schematic diagram of the discrimination process of the fruit tree row direction in the example of the present invention: showing the KDE density analysis of the angles of the nearest points.

[0038] Figure 8 This is the three-layer cost map of the orchard generated in the example of the present invention.

[0039] Figure 9 This is a schematic diagram of the 16-neighborhood used in the improved A* algorithm in the example of the present invention.

[0040] Figure 10 This is a schematic diagram of the path smoothing process used in the improved A* algorithm in the example of the present invention.

[0041] Figure 11 This is a schematic diagram of the paths generated by different path planning algorithms in the cost map in the example of the present invention. Specific embodiments

[0042] The following combines the accompanying drawings to further describe the specific embodiments of the invention. The following embodiments are only used to more clearly illustrate the technical solutions of the present invention and cannot be used to limit the protection scope of the present invention.

[0043] Figure 1 The flowchart of an orchard shortest path planning method based on a lidar SLAM map provided by an embodiment of the present invention is given. The steps included in this method are specifically as follows:

[0044] S01. Collect the three-dimensional point cloud data of the orchard environment using a lidar, and adopt the LIO-SAM (Lidar-Inertial Odometry and Simultaneous Localization and Mapping) algorithm. By optimizing the factors including LiDAR odometry factor, IMU pre-integration factor, GPS factor, and loop closure factor, achieve the construction of a high-precision three-dimensional point cloud map of the orchard. The lidar SLAM map constructed can accurately reflect the spatial distribution of the terrain, trees, and other obstacles in the orchard. The photos of the real orchard environment and the SLAM map constructed are as shown in Figure 2 and Figure 3 shown;

[0045] S02. Perform downsampling processing on the collected three-dimensional point cloud map to remove noise points and outliers and reduce the density of the point cloud data. Adopt a downsampling algorithm based on voxel grid (Voxel Grid). Divide the point cloud space into multiple small voxel grids, and calculate the average position of the points within each grid as the representative point of the grid. In this way, the data density can be significantly reduced while retaining the geometric structure and features of the point cloud;

[0046] S03. Use the cloth simulation filtering algorithm (CSF) to remove the ground point cloud from the downsampled point cloud data. The CSF algorithm separates the ground point cloud from the non-ground point cloud by simulating the drooping process of cloth under the action of gravity, so as to accurately extract the ground point cloud. Perform grid division on the extracted orchard point cloud and ground point cloud. The grid size is set according to the actual size and resolution requirements of the orchard. Calculate the average height of the ground point cloud within each grid, and screen out the orchard point cloud within the height range of the operating vehicle above the ground point cloud. This method fully considers the shape of the ground, making the height screening more scientific and reasonable, rather than simply retaining the point cloud within a certain height range above the horizontal line. By performing height screening on the orchard point cloud, not only the computing resources are greatly reduced, but also the interference of high-altitude tree branches is avoided. After completing the height screening of the point cloud, further obtain the slope information of the ground point cloud. For the ground point cloud within each grid, calculate the horizontal distance and height difference between it and the lowest point, and use the formula to calculate the slope value, convert the slope value to the slope angle, and calculate the average slope angle of each grid. These slope information will be used to create a slope cost map, providing an information basis for subsequent shortest path planning. The schematic diagram of the principle of the point cloud height screening and slope information calculation process is as shown in Figure 4 shown;

[0047] S04. Segment the highly screened point cloud data based on the improved random forest model to distinguish between passable areas and non-passable areas. First, extract the geometric features of the point cloud, including the position coordinates, normal vectors, and local density of the points. By calculating the local density of each point (such as the number of points per unit volume in the neighborhood of the point) and the normal direction, the model's perception of the point cloud features is enhanced. On this basis, the improved random forest model introduces a feature weighting mechanism, which evaluates the importance of each feature by training the initial model and weights the features according to the importance. This improvement enables the model to focus more accurately on features that are more critical to the classification task, thereby improving the classification accuracy. At the same time, in response to the problem of unbalanced proportions of passable areas and non-passable areas in point cloud data, the category weight adjustment technology is used to give higher weights to the passable areas (minority classes) to enhance the model's recognition ability of the minority classes. Finally, the orchard point cloud data marked as passable areas and non-passable areas is used to train the model. After the preprocessed point cloud data is input into the trained random forest model, the model outputs the classification result of each point, thereby generating a map reflecting the distribution of the orchard access area and obstacles. The specific segmentation effect is as follows Figure 5 This process not only improves the accuracy of point cloud segmentation, but also provides high-quality environmental information for subsequent path planning;

[0048] S05. After the orchard point cloud segmentation task is completed, in order to extract the center position information of the fruit tree to support subsequent path planning and operation tasks, the improved DBSCAN density adaptive clustering algorithm is used to extract the center point coordinates of the fruit tree from the segmented fruit tree point cloud. The algorithm dynamically adjusts clustering parameters (such as neighborhood radius ∈ and minimum number of neighborhood points MinPts) to adapt to the local density differences of the fruit tree point cloud in the orchard environment. The local point cloud density ρ is calculated by the Gaussian kernel density estimation method. p Based on local density analysis, the algorithm can dynamically adjust clustering parameters in low-density areas (ρ p Smaller) to expand the neighborhood range to avoid missed detection and reduce the MinPts threshold to enhance the sensitivity to sparse point clouds. p The larger the value is, the smaller the neighborhood is to prevent over-segmentation and the higher the threshold is to suppress noise interference, thus avoiding over-segmentation of high-density areas and missed detection of low-density areas. In addition, a joint criterion for noise points combining local density and deviation is proposed: the neighborhood density ρ of point p is calculated p , if p Below the threshold value θ ρ , then it is determined as a potential noise candidate point, and the minimum distance δ from point p to the nearest neighbor is further calculated. p =mind(p,p i ), if δ p Exceeding the threshold value θ δ, indicating that this point is far from the main cluster and is determined as a noise point. This method can effectively identify and filter out noise points that are isolated and deviate from the main cluster, thereby improving the accuracy of clustering. When extracting the center point of fruit trees, the present invention further adopts a weighted centroid calculation method based on local density, and assigns different weights to each point according to the local density of the point cloud. This method makes the contribution of high-density regions to the centroid greater, and the centroid coordinate c0 is determined by the weighted average of the points within the cluster C: where p is the point within the cluster, w p is the weight of point p, and the greater its local density, the greater the weight, so that the center point of the fruit tree can be extracted more accurately, and high robustness and accuracy can be maintained even in scenarios with complex occlusion and noise interference. The extraction effect of the center point of the fruit tree is as Figure 6 shown;

[0049] S06. In the orchard operation scenario, tree rows are the basis for constructing the orchard structure, and robots need to drive stably along the inter-row path during operation. Therefore, accurately discriminating the direction of tree rows is crucial for orchard navigation. In order to accurately discriminate the direction of fruit tree rows, the Kernel Density Estimation (KDE) technique is used to estimate the direction of tree rows. First, for each tree point P i =(x i , y i ) within the field of view, the nearest neighbor point P min <|P i -P in |<d max condition is screened, where in is the mean value of the plant spacing in the orchard. By this method, cross-row misconnection and too-close noise interference can be avoided. Connect points P i , P in in and calculate the angle between it and the x-axis and normalize the angle to the interval (-90°, 90°). KDE kernel density estimation is performed on the directions of the nearest neighbor points of all tree points, where the bandwidth h is dynamically calculated by the Silverman criterion. Compared with the fixed bandwidth, this strategy can adaptively adjust the width of the kernel function and improve the density estimation accuracy in sparse regions. The specific formula is: where K(θ) is the Gaussian kernel function, is the probability density function estimated at θ, n represents the number of samples, h represents the bandwidth, σ is the angle standard deviation, and R = Q3 - Q1 is the interquartile range. Calculate the angle θ l, that is, the estimated direction of the tree row is obtained. The schematic diagram of the process for judging the tree row direction is shown in Figure 7. Among them, Figure 7(a) shows the obtained center points of the fruit trees, Figure 7(b) shows the process of selecting the nearest neighbor points for each tree point in the field of view and connecting them with line segments. The line segments of different colors correspond to the corresponding tree points. Figure 7(c) shows the process of normalizing the angles of these target line segments to (-90°, 90°), and Figure 7(d) shows the KDE density analysis of the angles of the nearest points. The green dashed line represents the estimated maximum density point, that is, the final estimated direction of the tree row.

[0050] S07. To achieve efficient, safe and smooth shortest path planning in the orchard, a cost map that comprehensively considers the terrain slope and obstacle distribution is constructed, and an improved A* algorithm is used for path search. Finally, the path quality is further optimized through local smoothing processing, which specifically includes the following steps:

[0051] (1) Construct a cost map including a slope cost layer. As Figure 8 shown, the cost map consists of the following three main parts:

[0052] Static obstacle layer: The point cloud of the fruit trees obtained by segmentation is divided into grids. If the number of point clouds in the grid is greater than the threshold, it is judged as a static obstacle to form preliminary static obstacle information. However, at this time, the fruit trees are relatively independent, and it is easy to "cross" between the fruit trees during path planning. In order to form the organizational form of the tree row, it is also necessary to further adopt the method of fitting the fruit tree row line segments, convert the geometric features of the fruit tree row into line segment form, and incorporate them into the obstacle classification system. Specifically, first, the point cloud data is linearly fitted, and the least squares method is used to determine the slope and intercept of each fruit tree row. For the fitted lines, the distance between two lines is calculated to judge whether to retain the new fitted line segment. Given two lines L1: y = m1x + b1 and L2: y = m2x + b2, the distance between them When the distance between the new fitted line segment and the selected line segment is greater than the set threshold, it is added to the final set of fruit tree row line segments. This step can avoid the situation where the same fruit tree row is fitted multiple times. In addition, in order to remove the influence of outliers, this paper further introduces a line segment range calculation method based on the point cloud range. By calculating the distance from each point to the fitted line, the point clouds within the threshold range of the line distance are screened out, and the quartile method is used to remove the abnormal points, so as to obtain stable and representative fruit tree row line segments. The x-coordinate distribution is as shown in the formula: X valid∈[Q1 - 1.5IQR, Q3 + 1.5IQR], where Q1 and Q3 are the first and third quartiles, and IQR = Q3 - Q1 is the interquartile range. These line segments, as part of the static obstacle layer, together with other obstacles, constitute the static obstacle information layer of the cost map, helping the robot avoid fruit tree trunks and other obstacles, and can also ensure that the robot can strictly follow the layout of the fruit tree rows when planning the path, thus improving the accuracy and safety of path planning.

[0053] Obstacle inflation layer: Inflate the fruit tree point cloud and other obstacles to set a safety buffer around the obstacles to prevent the robot from getting too close to the obstacles.

[0054] Slope cost layer: Based on the previously obtained slope information, assign different cost values according to the slope magnitude. The larger the slope, the higher the cost, thus guiding the path to avoid steep slope areas.

[0055] (2) Improved A* algorithm

[0056] Based on the constructed cost map, use the improved A* algorithm for path planning. The specific improvements are as follows:

[0057] Cost function improvement: Introduce obstacle information and slope information into the cost function, and define a new cost function: G(neighbor) = G(current) + cost(move) + cost(map[i][j]), where cost(map[i][j]) represents the cost value of the cost map at the current location. In this way, the path planning will preferentially avoid high-slope areas to ensure the safety of the path.

[0058] Search method optimization: Use the 16-adjacency method instead of the traditional 4-adjacency or 8-adjacency search method to reduce the turning points in the path and make the path smoother. The 16-neighborhood is shown as Figure 9 shown. When calculating the real cost function g(n), adjust the movement cost between nodes according to the direction: the movement cost in the up, down, left, and right directions is 10; the movement cost in the diagonal direction is 14; the movement cost in the remaining directions is 22. This method not only improves the smoothness of the path but also reduces the redundant nodes in path planning.

[0059] Heuristic function improvement: In the calculation of the heuristic function f(n), add an additional value to preferentially select the nodes with less change in the path direction. When multiple nodes have the same f value, judge the degree of path direction change by calculating the cross product of the vector from the initial node to the target node and the vector from the current node to the target node.

[0060] Path safety enhancement: When planning a path, consider the relationships between nodes, avoid vertices with threat grids, and prevent the robot from passing obliquely through the vertices of obstacle grids, thereby improving the safety of the path.

[0061] (3) Path smoothing

[0062] To further optimize the smoothness of the path, local smoothing is performed on the generated path. Specifically, select the turning points in the path and the two midpoints formed by the two path points before and after the turning point as the smoothing points. Let the turning point be P1, the previous path point be P0, and the next path point be P2, then the interpolation points are P 01 、P 12 and P1, where Smooth interpolation is performed on the turning part through a quadratic Bézier curve. The parametric equation of the smooth curve is: B(t) = (1 - t) 2 P 01 + 2t(1 - t)P1 + t 2 P 12 . Finally, splice the smooth line segment with the unmodified original path segment to obtain a smooth and efficient shortest path. The process of path smoothing is shown as Figure 10 shown. The pink line segment is the path segment retained in the original path, the blue dashed line is the path segment optimized in the original path, and the red line segment is the optimized smooth line segment. Therefore, the new path is composed of the splicing of the pink line segment and the red line segment. This local smoothing not only improves the smoothness of the path but also retains the characteristics of the original path as much as possible, ensuring that the robot can travel efficiently and safely in a complex orchard environment.

[0063] The paths generated by three path planning algorithms under different starting point conditions in the cost map are shown as Figure 11 shown, where the path generated by the traditional A* algorithm is represented by a yellow line, the path generated by the initially improved A* algorithm based on different gray values is represented by a red line, and the path generated by the deeply improved A* algorithm after path smoothing is represented by a green line.

[0064] The above-described embodiments only represent several implementation manners of the present invention. Their descriptions are relatively specific and detailed, but they should not be construed as limiting the scope of the invention. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several deformations and improvements can still be made, which all belong to the protection scope of the present invention. The protection scope of the present invention is given by the appended claims and any equivalent technical solutions thereof.

Claims

1. An orchard shortest path planning method based on a lidar SLAM map, characterized in that The method includes the following steps: S01. Construct an orchard three-dimensional point cloud map: Use the LIO-SAM (Lidar-Inertial Odometry and Simultaneous Localization and Mapping) lidar SLAM construction technology to achieve high-precision orchard three-dimensional map construction by fusing the point cloud data of the lidar with the acceleration and angular velocity data of the inertial measurement unit (IMU); S02. Point cloud downsampling: Perform downsampling processing on the three-dimensional point cloud map to remove noise points and outliers and reduce the density of the point cloud data; The downsampling uses a method based on a voxel grid, divides the point cloud space into multiple small voxel grids, and calculates the average position of points within each grid to represent the point cloud of that voxel; S03. Point cloud height screening and slope information acquisition: Perform height screening on the downsampled point cloud data, extract the ground point cloud, and calculate the average slope angle within each grid to obtain slope information; The height screening uses the cloth simulation filtering algorithm (CSF) to separate the ground point cloud from the non-ground point cloud, and performs grid division on the separated orchard point cloud and ground point cloud, and filters out the orchard point cloud within the height range of the operating vehicle above the ground point; For the ground point cloud within each grid, calculate the horizontal distance and height difference between it and the lowest point, and use the formula to calculate the slope value to obtain the average slope angle of each grid. This slope calculation method will be used to create the subsequent slope cost map; S04. Fruit tree point cloud segmentation: Segment the point cloud data after height screening based on an improved random forest model to distinguish the passing area and the non-passing area; In the model training stage of the improved random forest model, by evaluating the importance of each feature and weighting it, the model can focus on key features; At the same time, aiming at the problem of unbalanced proportions of different regions in the point cloud data, a class weight adjustment technique is adopted to give higher weights to minority class samples; In addition, the key parameters of the random forest are optimized through cross-validation and grid search to further improve the generalization ability and accuracy of the model; S05. Fruit tree center point clustering: Cluster the segmented fruit tree point cloud to extract the center point coordinates of the fruit trees; Use an improved DBSCAN algorithm. First, dynamically adjust the clustering parameters using the local point cloud density to adapt to point cloud distributions with different densities; Second, dynamically identify and filter out noise points by combining the joint criterion of local density and deviation; Finally, use a density-weighted centroid calculation method to assign different weights to each point according to the local density of the point cloud, so that the high-density region contributes more to the centroid; S06. Fruit tree row direction discrimination: Calculate the direction of the tree row through kernel density estimation technology; adopt a tree row direction estimation method based on the angles of neighboring points. First, screen out the most neighboring points that meet the conditions for each tree point within the field of view, then connect each tree point to its most neighboring point, calculate the angle between the connecting line and the x-axis, and normalize the angle to the interval (−90°, 90°); further, perform kernel density estimation on the directions of the most neighboring points of all tree points, and dynamically calculate the bandwidth parameter of the kernel density estimation to adapt to point cloud distributions with different densities; finally, determine the estimated direction of the tree row by calculating the angle with the maximum density of the kernel density estimation. S07. Shortest path planning based on a cost map: Construct a cost map containing a slope cost layer, and combine it with an improved A* algorithm to generate the shortest path from the starting point to the ending point; the cost map consists of a static obstacle layer, a slope cost layer, and an inflation layer. The improved A* algorithm adds the cost information of the cost map to the cost function, thus effectively guiding the path to avoid high-slope areas and obstacles. In addition, use the 16-neighbor mode to replace the traditional 8-neighbor and 4-neighbor modes to increase the flexibility of the path, and add additional values to the heuristic function to preferentially select nodes with less change in the path direction; finally, perform local smoothing on the generated path, and perform smooth interpolation on the turning parts through quadratic Bezier curves to obtain a smooth and efficient shortest path.

2. The method according to claim 1, wherein In step S03: (3.1)The method for obtaining the height of the work vehicle is as follows: Pre-measure the lowest working height of the work vehicle and input this height value as a fixed parameter into the path planning system; this height value can be obtained through vehicle design parameters or on-site measurement to ensure its applicability to the orchard operation environment. The calculation formula described in (3.2) is , where is the point with the smallest z coordinate among all the ground point clouds within the current grid.

3. The method according to claim 1, wherein In step S07: (7.1)When constructing the static obstacle layer of the cost map, it is necessary to accurately identify and label the positions of the fruit tree point clouds. In addition, in order to form the organizational form of the tree row, it is necessary to fit the fruit tree row line segments by the least squares method and screen out the newly fitted line segments whose distances from the selected line segments are greater than the set threshold to ensure the accuracy and representativeness of the fruit tree row line segments, and then convert the geometric features of the fruit tree row into line segment form and incorporate them into the obstacle classification system. (7.2)When constructing the slope cost layer of the cost map, based on the ground slope information obtained by the cloth filtering algorithm, different cost values are assigned according to the slope size. The larger the slope, the higher the cost, so as to guide the path to avoid steep slope areas. (7.3)When constructing the inflation layer of the cost map, perform inflation processing on the obstacles to set a safety buffer around the obstacles to prevent the robot from getting too close to the obstacles. (7.4)Use the 16-neighbor mode to replace the traditional 4-neighbor or 8-neighbor search mode to optimize the smoothness of the path. At the same time, when planning the path, consider the relationship between nodes and avoid the vertices with threatening grids. (7.5)In the heuristic function, when multiple nodes have the same f value, judge the degree of path direction change by calculating the cross product of the vector from the initial node to the target node and the vector from the current node to the target node, so as to reduce unnecessary turns in the path. (7.6) Perform local smoothing on the generated path. Select the two midpoints formed by the turning point and the two path points before and after it as the smoothing points. Smoothly interpolate the turning part through a quadratic Bézier curve and splice the smoothed line segment with the original path segment that has not been modified to obtain a smooth and efficient shortest path.

Images