Agricultural inspection robot path planning method and system for mulberry orchard

Abstract

The present application relates to the technical field of path planning, in particular to a path planning method and system for agricultural inspection robot in mulberry orchard. The method comprises: constructing a grid map for the working area; fitting a first path in the grid map based on the end points of the travel area in the working area, and generating a plurality of first nodes on the first path; constructing a diffusion function based on the similarity measure calculated by the distance and the angle difference of tangent angle between the first nodes, and generating a diffusion coordinate map for the grid map using the diffusion function; planning a global working path for the travel area by A* algorithm with the weighted sum of the Euclidean distance of the grid map and the diffusion distance of the diffusion coordinate map as the heuristic function; after detecting a new obstacle in real time while traveling along the global working path, planning a local obstacle avoidance path between the current position and the next first node by Bi-AM-RRT algorithm with the diffusion distance guiding path expansion, which improves the tracking performance of path planning.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, specifically to a path planning method and system for an agricultural inspection robot used in mulberry orchards. Background Technology

[0002] To ensure operational safety and crop protection in mulberry orchards, inspection robots need to possess efficient and precise obstacle avoidance capabilities. Existing technologies, such as Chinese invention patent CN112393728A, disclose a mobile robot path planning method based on the A* and RRT* algorithms. This method takes the starting and ending coordinates of the mobile robot's current movement, discretizes the line segment connecting the starting and ending coordinates into N points, and then uses the A* and RRT* algorithms to plan the path based on these N discretized points.

[0003] Existing path planning algorithms typically assume flat terrain and use Euclidean distance calculated in Cartesian coordinates as a metric, considering only the spatial relationships of path nodes in Cartesian space. However, within the same farm road, path nodes that are spatially closer share adjacent terrain or crops, exhibiting higher spatial similarity; or they may be located on the same straight or curved section, resulting in greater consistency in the tangent direction of the farm road where the path nodes are located; furthermore, the direction of travel within the same farm road is usually consistent. Existing path planning algorithms struggle to consider the spatial similarity of path nodes and the consistency of the farm road's tangent direction, leading to problems such as path planning deviating from the farm road's centerline or crossing crop rows.

[0004] Therefore, there is an urgent need for a path planning method and system for agricultural inspection robots in mulberry orchards, which can improve the efficiency of path planning, guide the path along the direction of the farm road, and improve the tracking performance of the path planning. Summary of the Invention

[0005] The purpose of this invention is to address the technical problem in existing technologies where Euclidean distance calculated using Cartesian coordinates is used as the core metric, leading to path planning deviations from the centerline of the farm road and even crossing crop rows. This invention provides a path planning method and system for agricultural inspection robots in mulberry orchards. By introducing spatial similarity of path nodes and tangential similarity of the farm road, the method guides the path planning along the centerline of the farm road, improving the accuracy and tracking performance of the path planning.

[0006] To achieve the above-mentioned objectives, the present invention provides the following technical solution:

[0007] A path planning method for agricultural inspection robots in mulberry orchards, including

[0008] Construct a raster map of the work area; fit a first path in the raster map based on the endpoints of the travel area in the work area, and generate multiple first nodes on the first path;

[0009] A diffusion function is constructed based on the similarity metric calculated from the distance between the first nodes and the angle difference of the tangent angle. The diffusion function is then used to generate a diffusion coordinate map from the raster map.

[0010] The A* algorithm, which uses the weighted sum of the Euclidean distance of the raster map and the diffusion distance of the diffusion coordinate map as the heuristic function, plans the global operation path for the travel area.

[0011] After detecting new obstacles in real time while traveling along the global operation path, a local obstacle avoidance path is planned between the current position and the next first node using the Bi-AM-RRT algorithm, which guides the path expansion by diffusion distance.

[0012] This application also provides a path planning system for an agricultural inspection robot in a mulberry orchard, including:

[0013] The map module is used to construct a raster map of the work area; based on the endpoints of the travel area in the work area, a first path is fitted in the raster map, and multiple first nodes are generated on the first path;

[0014] The transformation module is used to construct a diffusion function based on the similarity metric calculated from the distance between the first nodes and the angle difference of the tangent angle, and to generate a diffusion coordinate map for the raster map using the diffusion function;

[0015] The global planning module is used to plan the global operation path for the travel area using the A* algorithm, which uses the weighted sum of the Euclidean distance of the raster map and the diffusion distance of the diffusion coordinate map as the heuristic function.

[0016] The local obstacle avoidance module is used to plan a local obstacle avoidance path between the current position and the next first node by using the Bi-AM-RRT algorithm, which guides the path expansion by diffusion distance, after detecting new obstacles in real time while traveling along the global operation path.

[0017] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0018] 1. An improved A* algorithm is used to plan a global operation path in the mulberry orchard, proactively avoiding static obstacles. Upon detecting newly appearing obstacles, an improved Bi-AM-RRT algorithm is used to plan a local obstacle avoidance path based on nodes on the global operation path. After completing local obstacle avoidance, the path can revert to the global operation path, making the local obstacle avoidance path closer to the global operation path. This allows the inspection robot to operate efficiently along the pre-planned global operation path while avoiding dynamically appearing unknown obstacles. Simultaneously, based on the spatial similarity and tangential direction similarity of the first node of the farm road, a diffusion coordinate map is generated for the mulberry orchard's grid map in Cartesian coordinate space. Both the improved A* and Bi-AM-RRT algorithms incorporate diffusion distance calculations in the diffusion coordinate calculations, ensuring that the planned path nodes possess both optimal spatial distance and spatial similarity, as well as directional similarity to the farm road. This improves the accuracy of path planning and the tracking performance along the farm road.

[0019] 2. In the process of global path planning and local path planning, Cartesian coordinates are used to calculate Euclidean distance to represent spatial distance in order to search for the nearest node; diffusion coordinates are introduced to calculate the diffusion distance to represent the spatial similarity and tangential similarity between nodes, which is beneficial for searching nodes in the same farm road, so as to guide the path to expand along the direction of the farm road, and improve the tracking performance during path planning.

[0020] 3. Nodes that are spatially close along the same tangent direction of the farm road have high similarity. The spatial similarity of nodes is quantified by the distance between nodes in the mulberry orchard, and the similarity of the farm road tangent direction is quantified by the angle difference of the tangent angle. The similarity measure between nodes is calculated based on spatial similarity and tangent direction similarity. The diffusion function constructed based on the similarity measure generates a diffusion coordinate space in the Cartesian coordinate space of the first grid map, which is equivalent to compressing the distance in the direction perpendicular to the farm road, thus increasing the probability that the nearest tree node and the sampled node are located on the same or adjacent farm road. Attached Figure Description

[0021] Figure 1 A schematic diagram of the structure of an agricultural inspection robot path planning system for mulberry orchards provided in this application;

[0022] Figure 2 A flowchart illustrating a path planning method for an agricultural inspection robot used in mulberry orchards, as provided in this application;

[0023] Figure 3 This is a schematic diagram illustrating the process of constructing a diffusion function based on spatial similarity and tangent direction similarity. Detailed Implementation

[0024] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. It should be noted that, unless otherwise specified, the embodiments and features and technical solutions in the embodiments of the present invention can be combined with each other.

[0025] It should be noted that similar reference numerals and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures. Furthermore, the terms "first," "second," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0026] For example Figure 1 The diagram shown is a structural schematic of a path planning system for an agricultural inspection robot used in a mulberry orchard, provided in this embodiment. In one embodiment of this application, it includes:

[0027] The map module is used to construct a raster map of the work area; based on the endpoints of the travel area in the work area, a first path is fitted in the raster map, and multiple first nodes are generated on the first path;

[0028] The transformation module is used to construct a diffusion function based on the similarity metric calculated from the distance between the first nodes and the angle difference of the tangent angle, and to generate a diffusion coordinate map for the raster map using the diffusion function;

[0029] The global planning module is used to plan the global operation path for the travel area using the A* algorithm, which uses the weighted sum of the Euclidean distance of the raster map and the diffusion distance of the diffusion coordinate map as the heuristic function.

[0030] The local obstacle avoidance module is used to plan a local obstacle avoidance path between the current position and the next first node by using the Bi-AM-RRT algorithm, which guides the path expansion by diffusion distance, after detecting new obstacles in real time while traveling along the global operation path.

[0031] In another embodiment, the map module further includes a positioning submodule for acquiring satellite signals and outputting centimeter-level positioning information using RTK-GNSS high-precision positioning technology. The main antenna and slave antenna of the RTK-GNSS module are respectively mounted on both sides of the robot, with an initial lateral distance of 0.5m between them.

[0032] In another embodiment, the local obstacle avoidance module further includes an environmental acquisition submodule, used to acquire an RGB image of the mulberry orchard environment and a depth map representing the distance between the environment and the robot using a depth camera, acquire a point cloud using a LiDAR, and perform spatial and temporal alignment of the RGB image, depth map, and point cloud using a checkerboard calibration board. In this embodiment, a mechanical multi-line LiDAR is selected, initially installed at a height of 1.0~1.5m above the ground to accommodate both crop canopy and ground obstacle detection, with a scanning frequency set to 20Hz. The depth camera is an RGB-D camera, installed at the front of the robot, below the LiDAR mounting plate.

[0033] In another embodiment, a calibration module is also included, which is used to complete the extrinsic parameter calibration of the LiDAR and depth camera using a checkerboard calibration board before constructing the grid map, to ensure that the collected point cloud is aligned with the coordinate space of the depth camera image; and to receive manually marked key locations in the mulberry orchard, such as the endpoints of the farm road and mulberry tree rows, to complete the absolute calibration of the RTK-GNSS.

[0034] For example Figure 2 The diagram shown is a flowchart illustrating a path planning method for an agricultural inspection robot in a mulberry orchard, as provided in this embodiment. The method includes:

[0035] Construct a raster map of the work area; fit a first path in the raster map based on the endpoints of the travel area in the work area, and generate multiple first nodes on the first path;

[0036] A diffusion function is constructed based on the similarity metric calculated from the distance between the first nodes and the angle difference of the tangent angle. The diffusion function is then used to generate a diffusion coordinate map from the raster map.

[0037] The A* algorithm, which uses the weighted sum of the Euclidean distance of the raster map and the diffusion distance of the diffusion coordinate map as the heuristic function, plans the global operation path for the travel area.

[0038] After detecting new obstacles in real time while traveling along the global operation path, a local obstacle avoidance path is planned between the current position and the next first node using the Bi-AM-RRT algorithm, which guides the path expansion by diffusion distance.

[0039] Specifically, in this embodiment, the work area corresponds to a mulberry orchard, including the travel area corresponding to the farm roads between rows of mulberry trees and the obstacle area corresponding to obstacles, including but not limited to rows of mulberry trees, fixed work equipment, and monitoring equipment. A grid map corresponds to the spatial environment modeling of the mulberry orchard. In this embodiment, RTK-GNSS positioning technology is used to collect key locations in the work area to establish boundary constraints, such as the endpoints of the farm roads and rows of mulberry trees. These key locations are identified by the operators and include at least the corner vertices of the work area, the endpoints of the farm roads, and the corner locations of obstacles. A grid map, the first grid map, is generated based on these key locations in the work area using the RTK-GNSS tool library. The first grid map is labeled with the farm roads used for the inspection robot's movement, the rows of mulberry trees representing the mulberry planting area, and other fixed obstacles. The start and end points of each farm road are marked according to the preset work direction. It should be noted that when the farm roads and rows of mulberry trees in the mulberry orchard remain unchanged, the global map of the mulberry orchard plan for the first operation can be saved in the database and retrieved directly from the database when operating on the same mulberry orchard in subsequent operations.

[0040] Furthermore, in the first grid map, the midpoints of both ends of the farm road are used as the start and end points. A first path, representing the straight-line reference direction of the farm road, is fitted to the start and end points using a straight-line fitting function to reflect the overall direction of the farm road. Between the start and end points of the first path, the first path is discretized, generating multiple first nodes by marking one node at every first interval. In this embodiment, the first interval is set to 5m. Workers mark key locations on the first path, such as the endpoints of the farm road, turning points, crop row crossing points, and obstacle locations, and these are also marked as first nodes. Multiple first nodes are added to an array in a preset order according to the direction of travel to form a first node sequence.

[0041] Furthermore, taking the robot's current position as the starting node and the first non-planned node with the smallest Euclidean distance to the current position as the target node, the A* algorithm is used to plan the global operation path for the farm road. The A* algorithm calculates the total cost between the coordinates of the inspection robot's current position and the target node using the actual cost and the estimated cost as heuristic functions. The actual cost is the Euclidean distance between the current path starting point (i.e., the i-th first node in the first node sequence) and the inspection robot's current position; the estimated cost is the Euclidean distance between the inspection robot's current position and the target node (i+1-th first node). New path nodes are expanded between the current position and the target node according to the total cost, guiding the path search to converge towards the target node, and finally planning the shortest path from the start to the end of the farm road. However, since the A* algorithm only considers the shortest spatial path when calculating the Euclidean distance, it defaults to expanding to all adjacent feasible nodes. If the direction of travel of the first path is not incorporated when expanding new path nodes, the accumulation of multiple expansions may cause the global operation path to deviate significantly from the centerline of the farm road, thus reducing the efficiency of path planning between subsequent nodes.

[0042] Since mulberry orchards typically include multiple farm roads, the inspection robot travels in a back-and-forth pattern along these roads, meaning the end of one road serves as the starting point for an adjacent road, resulting in opposite directions of travel between adjacent roads. Furthermore, farm roads in mulberry orchards often have curves, creating tangent angles between the curves and the endpoints of the roads. In the environmental space, closer first nodes indicate higher spatial similarity; for example, adjacent first nodes might share the same mulberry tree or the same soil. A smaller difference in the tangent angle between first nodes indicates a more consistent tangent direction between their farm roads. Using the spatial similarity of first nodes and the similarity of their farm road tangent directions as a similarity metric is beneficial for guiding the global operation path along the first path during path planning.

[0043] Therefore, in this embodiment, based on the Cartesian coordinate space of the original first grid map, the angle difference of the tangent angle between the first nodes and the Euclidean distance between the first nodes are calculated. Based on the Euclidean distance and the angle difference, the similarity measure between the first nodes is calculated. Based on the similarity measure, a diffusion function is constructed to map the Cartesian coordinate space of the first grid map into a diffusion coordinate map representing the spatial similarity and tangent direction similarity between the first nodes.

[0044] Furthermore, when executing the A* algorithm, the Euclidean distance between the current position of the inspection robot and the Cartesian coordinates of the target node, as well as the diffusion distance of the diffusion coordinates, are calculated using the Euclidean distance formula. The heuristic function is set as a weighted sum of the Euclidean distance and the diffusion distance between the current position of the inspection robot and the target node, making the path planning more inclined to expand the path along the direction of the farm road, thus improving the tracking performance of the global operation path along the direction of the farm road. The calculation formula is as follows:

[0045] ;

[0046] Where f is the weighted sum of Euclidean distance and diffusion distance; h is the Euclidean distance between the current position and the target node's Cartesian coordinates; g is the diffusion distance between the current position and the target node's diffusion coordinates; a and b are weight coefficients, and in this embodiment, the weight ratio of a to b is set to 7:3. The A* algorithm, with an improved heuristic function, plans paths between adjacent first nodes in the first node sequence along the direction of travel. The planned paths are then smoothed using cubic spline interpolation to obtain the global operation path along the farm road.

[0047] When the inspection robot is operating, it first travels along a statically planned global operation path. During this process, the rate of change of the tracked chassis's steering angle is ensured to be less than or equal to 10 degrees per second to minimize rapid changes in steering angle during movement. Since there are newly appearing dynamic obstacles in the mulberry orchard, such as workers or other operating robots, real-time obstacle detection is required during movement to trigger local path planning for obstacle avoidance. In this embodiment, the moving speed and distance from the current position of obstacles are detected in real-time using lidar ranging to determine whether the obstacle is static or dynamic. Dynamic obstacles are considered new obstacles; for static obstacles, it is determined whether the corresponding grid in the first grid map has an obstacle marker. If not, it is considered a newly added static obstacle, and local path planning is also required.

[0048] Furthermore, in another embodiment, since it is difficult to manually mark obstacles in the real-time acquired environmental images during the operation, and to determine whether obstacles are moving, this embodiment also includes: scanning with a LiDAR at 20Hz to acquire a point cloud in front of the inspection robot in real time, and determining whether the obstacle is static or dynamic based on the size and movement speed of the dense point cloud; acquiring RGB and depth maps in front of the inspection robot using a depth camera, and using a YOLOv8 model to identify obstacle categories from the environmental images, such as static crop rows and dynamic workers, and then marking the grids containing obstacles in the grid map.

[0049] Specifically, when the inspection robot operates along the global operation path, it inputs the point cloud set into the DBSCAN algorithm, setting the neighborhood radius, minimum number of points, and maximum number of iterations. For the first point cloud traversed, if the neighborhood of the first point cloud contains at least the minimum number of other point clouds, the first point cloud is marked as a core point. Based on density reachability, the core point and its reachable points are clustered into point cloud blocks. In this embodiment, the size of the point cloud block includes the horizontal width and vertical height measured along the direction perpendicular to the farm road. In other embodiments, it may also include the thickness of the point cloud block along the direction parallel to the farm road, calculated based on the depth map. Point cloud blocks with a vertical height greater than 30 cm and a horizontal width greater than 20 cm are marked as obstacles. In one embodiment, it further includes: when the lidar detects a new obstacle, calculating the distance between the obstacle and the inspection robot using lidar ranging technology. and relative velocity; if distance Obstacles less than 5 meters away and with a relative speed greater than 0 are classified as dynamic obstacles; if the distance... Obstacles less than 3 meters in distance and with a relative speed of 0 are classified as static obstacles.

[0050] In this embodiment, the Bi-AM-RRT algorithm is a Bi-RRT (Bidirectional Fast Expanding Random Tree) algorithm that guides path expansion based on diffusion distance. Local path planning is performed using the Bi-AM-RRT algorithm to obtain a local obstacle avoidance path. The Bi-RRT algorithm selects the nearest neighbor node in the tree based on Euclidean distance calculated in Cartesian coordinate space, and expands new tree nodes. However, the local obstacle avoidance path planned by the Bi-RRT algorithm may not be the shortest path. To ensure that the planned local obstacle avoidance path also tends to follow the first path, an improved Bi-AM-RRT algorithm is adopted. The current position of the inspection robot is used as the starting node, and the first node behind the obstacle in the direction of travel with the smallest Euclidean distance from the current position is used as the target node. Dual-tree expansion is performed at the starting node and the target node. When selecting the nearest neighbor node and expanding new tree nodes, diffusion coordinates representing the spatial similarity between tree nodes and the tangential similarity of the farm road are used. The diffusion distance is calculated using the Euclidean distance formula, guiding the obstacle avoidance path to tend to follow the first path, thus improving the tracking performance of the local obstacle avoidance path along the farm road direction.

[0051] The inspection robot first travels along the global operation path. When it identifies obstacles in the mulberry orchard in real time, it avoids the obstacles using a local obstacle avoidance path before returning to the global operation path. The task ends when the inspection robot's RTK-GNSS positioning reaches the last node in the first node sequence, with a positioning accuracy of less than or equal to 0.5m, and the ratio of the number of connected global and local obstacle avoidance paths to the total number of first nodes is greater than or equal to 95%.

[0052] An improved A* algorithm is used to plan a global operation path in the mulberry orchard, proactively avoiding static obstacles. Upon detecting newly appearing obstacles, an improved Bi-AM-RRT algorithm is used to plan a local obstacle avoidance path. After completing local obstacle avoidance, the robot reverts to the global operation path. This allows the inspection robot to operate efficiently along the pre-planned global operation path while avoiding dynamically appearing unknown obstacles. Simultaneously, based on the spatial similarity and tangent direction similarity of the first node of the farm road, a diffusion coordinate map is generated from the grid map of the mulberry orchard in Cartesian coordinate space. Both the improved A* and Bi-AM-RRT algorithms incorporate diffusion distance calculations in the diffusion coordinates, ensuring that the planned path nodes possess both optimal spatial distance and spatial similarity, as well as directional similarity to the farm road. This improves the accuracy of path planning and the tracking performance along the farm road.

[0053] In the process of global path planning and local path planning, Cartesian coordinates are used to calculate Euclidean distance, which represents spatial distance, to search for the nearest node; diffusion coordinates are introduced to calculate the diffusion distance, which represents the spatial similarity and tangential similarity between nodes. This is beneficial for searching for nodes in the same farm road, so as to guide the path to expand along the direction of the farm road and improve the tracking performance during path planning.

[0054] In one embodiment of this application, a diffusion function is constructed based on a similarity metric calculated from the distance between the first nodes and the angle difference of the tangent angle, for example... Figure 3 As shown, it includes:

[0055] The similarity metric is calculated based on the distance between the first nodes and the angle difference of the tangent angle, and a similarity matrix is ​​constructed using the similarity metric as elements;

[0056] Construct a degree matrix by using the sum of the elements in each row of the similarity matrix as the element values ​​of the corresponding row on the main diagonal of the diagonal matrix; multiply the transpose of the degree matrix with the similarity matrix to obtain the diffusion matrix.

[0057] Perform eigenvalue decomposition on the diffusion matrix, sort it from largest to smallest to obtain multiple eigenvalues ​​and corresponding eigenvectors, and construct the diffusion function based on the eigenvalues ​​and eigenvectors.

[0058] Specifically, the similarity metric between the first nodes is calculated, and a similarity matrix W is constructed based on this metric. The sequence of first nodes is obtained, and the tangent angle between the tangent direction of the first node on the farm road and the straight line connecting the endpoints of the farm road is calculated. The Euclidean distance between all first nodes is calculated using Cartesian coordinates to represent their distance similarity. The angle difference of the tangent angles between all first nodes, in radians (rad), is calculated to represent their similarity in the tangent direction of the farm road, ensuring that nodes with the same direction are connected firstly during path planning. The product of the Gaussian kernel function of the distance between each pair of first nodes and the Gaussian kernel function of the tangent angle is calculated to obtain the similarity metric between the first nodes. The calculation formula is as follows:

[0059] ;

[0060] Where i and j are the indices of the first node; The elements of the similarity matrix W represent the similarity between the i-th and j-th first nodes in the first node sequence. , Let be the diffusion coordinates of the i-th and j-th first nodes; n is the number of first nodes; and This represents the tangent angle of the farm road at the i-th first node and the j-th first node; The variance parameter of the spatial Euclidean distance of all first nodes is used to adjust the similarity weight of the Euclidean distance between first nodes; The variance parameter is the difference in angles between the tangent angles of all first nodes, used to adjust the similarity weights for the differences in tangent directions between first nodes.

[0061] A diffusion function is constructed based on the similarity matrix. The degree matrix D is constructed by using the sum of the elements in each row of the similarity matrix as the element values ​​of the corresponding row on the main diagonal of the diagonal matrix. The calculation formula is as follows:

[0062] ;

[0063] Transpose the degree matrix Multiplying the similarity matrix W by the diffusion matrix P yields the diffusion matrix P, calculated using the following formula:

[0064] ;

[0065] Perform eigenvalue decomposition on the diffusion matrix P, remove trivial eigenvectors and their corresponding eigenvalues, and arrange the remaining eigenvalues ​​in descending order to obtain the eigenvalue sequence. mm is the feature index. Let be the m-th eigenvalue; the corresponding eigenvector sequence obtained by sorting the eigenvalue sequence is: , Let x be the m-th eigenvector. The first d eigenvalues ​​are selected from the eigenvalue sequence in descending order, and the first d non-trivial eigenvectors are selected from the corresponding eigenvector sequence to construct a diffusion function. This function is used to apply the diffusion to each first node x in the first grid map. i The Cartesian coordinates are mapped to diffusion coordinates. The calculation formula is as follows:

[0066] ;

[0067] in, Let be the diffusion coordinates of the i-th first node; t is the preset diffusion time parameter; and d is the number of non-trivial feature vectors selected. , The first and dth eigenvalues ​​are raised to the power of t. , This is the i-th component of the 1st and d-th eigenvectors; i is the index of the first node; in this embodiment, d is set to 2 for planar coordinates and 3 for three-dimensional coordinates. A diffusion function is used to map the coordinates of all grid cells in the first grid map to their corresponding diffusion coordinates, resulting in a diffusion coordinate map corresponding to the first grid map. The eigenvectors represent the spatial similarity and tangential similarity of the farm road; the eigenvalues ​​represent the strength of the similarity. The larger the eigenvalue, the greater the spatial and tangential similarity, and the higher the efficiency of guiding the inspection robot along the farm road.

[0068] In one optional implementation, after mapping the diffusion coordinate map, the method further includes calculating the diffusion distance between any two first nodes and storing the diffusion distance in a lookup table or array, so that it can be directly retrieved when performing global path planning or local path planning later.

[0069] Nodes that are spatially close along the same tangent direction of the farm road have high similarity. The spatial similarity of nodes is quantified by the distance between nodes in the mulberry orchard, and the similarity of the farm road tangent direction is quantified by the angle difference of the tangent angle. The similarity measure between nodes is calculated based on spatial similarity and tangent direction similarity. The diffusion function constructed based on the similarity measure generates a diffusion coordinate space in the Cartesian coordinate space of the first grid map, which is equivalent to compressing the distance in the direction perpendicular to the farm road, thus increasing the probability that the nearest tree node and the sampled node are located on the same or adjacent farm road.

[0070] In one embodiment of this application, a local obstacle avoidance path is planned using the Bi-AM-RRT algorithm, which guides path expansion by diffusion distance, including:

[0071] The Bi-RRT algorithm is used for random tree growth. When searching for tree nodes, the nearest tree node with the smallest diffusion distance to the sampled node is obtained. When expanding a new tree node, if there is no obstacle between the nearest tree node and the sampled node, the sampled node is used as the new tree node. If there is an obstacle between the nearest tree node and the sampled node, the position of the preset step size is intercepted from the nearest tree node towards the sampled node based on the diffusion distance as the new tree node, and the new tree node is connected to the random tree.

[0072] Specifically, this embodiment uses the Python tool library to implement the Bi-AM-RRT algorithm. The RTK positioning coordinates of the current position of the inspection robot are set as the starting node, and the first node with the smallest Euclidean distance to the current position is set as the target node. In the first grid map, a grid with a preset area of ​​10m*10m and a resolution of 0.1m centered on the current position of the inspection robot is used as a local grid map. Sampling nodes are generated using the Sample_state function, and the Euclidean distance between the sampling node and the tree node is greater than or equal to 0.5m. If the projected area of ​​an obstacle in the local grid map is greater than 0.8 times the area of ​​the local grid map, the first node with the smallest Euclidean distance to the current position is directly used as the sampling node; if the projected area is less than or equal to 0.8 times the area of ​​the local grid map, the obstacle side with a larger vertical distance to the boundary of the farm road and a vertical distance greater than the width of the inspection robot's track chassis, and the farm road area where the inspection robot faces at an angle of less than 20 degrees to the farm road are selected as the obstacle avoidance area.

[0073] The random tree consists of a forward tree rooted at the current position and a reverse tree rooted at the first node with the smallest Euclidean distance to the current position. The forward and reverse trees alternately expand towards each other to create new tree nodes. It's important to note that inspection robots typically cannot move directly laterally; therefore, when expanding tree nodes, it's necessary to generate an arc path that adapts to the robot's turning constraints.

[0074] A sampling node is randomly selected within the obstacle avoidance area. The Nearest(T, x_rand) function is used to search for the nearest tree node in the current tree with the smallest diffusion distance to the sampling node, where T is the current tree and x_rand is the sampling node. The grid corresponding to the line connecting the sampling node and the nearest tree node is checked for obstacle markers. If there are no obstacles between the sampling node and the nearest tree node and the track steering constraint is satisfied, the connection is made directly. The track steering constraint formula is as follows:

[0075] ;

[0076] Where R is the turning radius; L is the track wheelbase; The turning angle is denoted by , with a maximum value of 30°. The turning angle is the angle between the inspection robot's direction of travel and the straight line of the farm road detected by the Hough transform.

[0077] If there is an obstacle between the sampling node and the nearest tree node, in the Steer(x_nearest, x_rand) function, the movement step is calculated based on the diffusion distance between the nearest tree node and the sampling node. The position of this movement step from the nearest tree node is the new tree node, where x_nearest is the nearest tree node. The turning angles of the nearest and new tree nodes are obtained, and the difference between the turning angles is calculated. This difference is then substituted into the track steering constraint formula to calculate the turning radius. If the difference in turning angles is greater than 30°... o If the turning radius is less than the minimum turning radius, increase the movement step and regenerate a new tree node; if the difference in turning angles is less than or equal to 30 degrees... o Furthermore, the turning radius is greater than or equal to the minimum turning radius. Using the turning radius, the length between the nearest and new tree nodes as the chord length, and the diffusion coordinates of these two tree nodes, an arc path is generated between them, connecting the nearest and new tree nodes. In this embodiment, the tracked chassis requires a minimum movement step of 0.5 meters for terrain adaptability, to reduce track slippage caused by frequent small movements.

[0078] After each connection of a new tree node in a forward or reverse tree, the Euclidean distance between the new tree node and the tree nodes in the other tree is calculated. If there exists a minimum Euclidean distance less than or equal to the two-tree connection threshold, and the path formed by connecting the tree nodes corresponding to the minimum Euclidean distance is free of obstacles, and the difference in the turning angles of the corresponding tree nodes is less than 15, then the connection is considered complete. o The reverse tree nodes are merged into the forward tree in reverse order. After merging the forward and reverse trees, a local obstacle avoidance path is obtained. In this embodiment, the dual-tree connection threshold is set to 2.5 times the width of the tracked chassis, ensuring that the path when the dual trees are merged has passability.

[0079] In local path planning, the Bi-RRT algorithm introduces diffusion distance search tree nodes and extended tree nodes to generate a new Bi-AM-RRT algorithm, which makes the randomly searched sampled nodes and the nearest tree nodes have spatial similarity and tangential direction similarity. The new tree nodes generated based on the diffusion distance between the nearest tree node and the sampled node will tend to extend along the direction of the farm road where the nearest tree node is located, which is beneficial to improving the tracking performance of the local obstacle avoidance path.

[0080] In one embodiment of this application, the method further includes: when the inspection robot moves to the next path node, the random tree with the current position as the root node is shifted to the next path node accordingly; if the number of root nodes shifted each time is less than or equal to the first node threshold, the neighboring nodes of the new root node are searched and updated; if the number of shifted root nodes is greater than the first node threshold, the random tree node relationship is updated by root reconnection optimization based on the Euclidean distance between the previous root node and the target node.

[0081] Specifically, in this example, the first node threshold is preset to 2. A root queue is created to temporarily store the offset root nodes. Local path planning is a dynamic process. While the inspection robot moves along the local obstacle avoidance path, it also plans the next path node until it completes obstacle avoidance and returns to the global operation path. After the inspection robot moves to the next path node along the local obstacle avoidance path, the root node of the random tree will shift to the next path node, that is, the current position of the inspection robot changes, and the node relationship of the random tree needs to be reconstructed. Therefore, after the inspection robot shifts to the new root node, the list of neighboring nodes of the original root node is cleared, and the new root node is added to the root queue. If the number of root nodes accumulated in the root queue each time is less than or equal to 2, with the new root node as the center, within the preset neighborhood radius, a search is conducted in the random tree for nodes whose Euclidean distance from the new root node is less than the distance threshold, are along the direction of the farm road, and have a turning angle less than or equal to 20 degrees. o The neighboring tree nodes are added to the neighboring node list. If the number of root nodes accumulated in the root queue is greater than 2 each time, root reconnection optimization is triggered, which means that root reconnection optimization is performed every two root nodes. A random tree based on the new root nodes is obtained, and the node relationships of the random tree are updated to reduce the overall path cost, including: dequeuing two root nodes from the root queue ( , ), That is, the previous root node, in the root node In the list of neighboring nodes, search for the neighboring node with the smallest Euclidean distance to the target node and that satisfies the track steering constraint. Set the neighboring node as the parent node of the target node. Update the node relationship of the random tree through the Update_edge function so that the new path adapts to the position offset of the inspection robot.

[0082] In one embodiment of this application, after expanding new tree nodes in the random tree, the method further includes: setting a second node threshold; if the cumulative number of new root nodes expanded each time is greater than the second node threshold, constructing a reconnection ellipse with the new tree node and the target node as the focus and the major axis parallel to the direction of the farm road, triggering target reconnection optimization to search for the tree node with the smallest diffusion distance from the target node in the reconnection ellipse, which is recorded as the target reconnection node; and generating a new path between the target reconnection node and the target node.

[0083] Specifically, in this example, the threshold for the second node is preset to 2. Target reconnection optimization is typically applied to forward trees, including: when a first node is expanded in the forward tree as a new tree node, this first node is added to the target stack. After new tree nodes are generated in the forward tree, target reconnection optimization is required every two new tree nodes expanded. If the number of first nodes in the target stack is greater than 2, two first nodes are popped from the stack; a reconnection ellipse region is constructed with the new tree node and the target node as the focus, with the major axis parallel to the direction of the farm road, to promote the generation of new paths along the direction of the farm road and improve the alignment between the path direction and the direction of the farm road. Within the reconnected ellipse, search the forward tree for the node with the smallest diffusion distance to the target node, and denote it as the target reconnected node. Generate a new path between the target reconnected node and the target node. If there is a branch in the forward tree whose diffusion distance to the target node is greater than the indirect path distance, clear that branch to avoid invalid exploration. The indirect path distance is the sum of the Euclidean distance from the root node to the current position and the diffusion distance from the current position to the target node, which is used to represent the estimated distance to reach the target node through other paths.

[0084] In some implementations, the maximum value of both the root reconnection time and the target reconnection time is set to 0.1 seconds. If the reconnection time is greater than 0.1 seconds, the reconnection optimization is terminated to match the steering response speed of the tracked chassis.

[0085] In the mulberry orchard scenario, the ground is the largest continuous surface. The ground is typically undulating, causing differences in the height of the collected point clusters, resulting in vertical undulations in the actual path traveled by the inspection robot. The first grid map constructed using RTK-GNSS for path planning is usually a continuous plane, making it difficult to reflect changes in ground slope. Therefore, the global operation path and local obstacle avoidance path planned in the first grid map are affected by ground undulations, leading to significant path deviations between the actual and planned paths during travel. Therefore, in one embodiment of this application, the construction of the grid map includes:

[0086] Multiple point cloud fitting plane models are obtained from the collected point cloud set. Dynamic thresholds are adaptively calculated based on the slope angle of the plane models. The RANSAC plane fitting algorithm for point clouds is used to filter the point clouds by using dynamic thresholds. Ground models are then fitted to the point cloud set collected in the work area, and the ground models are rasterized into a raster map.

[0087] Specifically, we first define a three-dimensional planar model as follows:

[0088] ;

[0089] Where (A, B, C) are the normal vectors of the planar model, and D is a constant representing the distance from the origin to the planar model. A minimum sample set of three point clouds is randomly selected from the point cloud set and substituted into the planar model formula to obtain candidate planar models. The slope angle is calculated using the angle between the normal vector u = (A, B, C) and the vertically upward vector v = (0, 0, 1). :

[0090] ;

[0091] The classic RANSAC plane fitting algorithm uses a fixed threshold to filter point clouds. However, point clouds located on slopes, which are far from the current position of the inspection robot, will have a large vertical distance relative to the poorly fitted plane model. Therefore, these distant point clouds are easily misclassified as non-ground points. Thus, in this embodiment, an adaptive dynamic threshold T is determined based on the slope angle.

[0092] ;

[0093] in, The base threshold is set to 0.1 meters; k is the slope coefficient, set to 0.1; and ω is the slope angle, expressed in radians. The distance from all point clouds to the planar model is calculated, and point clouds with distances less than the dynamic threshold T are marked as candidate point clouds. To avoid an excessively large dynamic threshold on extremely steep slopes, an upper limit T is set for the dynamic threshold. max It is 1 meter.

[0094] After iterating multiple times and fitting multiple planar models, the planar model with the most candidate point clouds is selected as the ground model. The corresponding candidate point clouds are then used as the ground point clouds. Outliers are removed from the ground point clouds using a statistical filtering algorithm. The resolution is set to 0.1 meters, and the ground model is rasterized to obtain a raster map. The number of iterations, N, is as follows:

[0095] ;

[0096] Where p is the success probability of the algorithm; w is the proportion of candidate point clouds to the number of point cloud sets; both p and w are set according to historical fitting results; m is the minimum number of samples required to construct the planar model, which is set to 3 in this embodiment; w m This represents the probability that all m randomly selected point clouds are candidate point clouds.

[0097] The RANSAC plane fitting algorithm uses a dynamic threshold for selecting point clouds based on slope angle adaptive determination. The dynamic threshold is proportional to the slope angle of the currently fitted plane model. The steeper the slope, the larger the dynamic threshold, which makes the point cloud on the steep slope more likely to be selected as the ground point cloud. This allows the final fitted ground model to reflect the differences in ground undulation to a certain extent. Path planning on this ground model is beneficial to improving the accuracy of path planning.

[0098] The above embodiments are only used to illustrate the present invention and are not intended to limit the technical solutions described herein. Although the present invention has been described in detail with reference to the above embodiments, the present invention is not limited to the specific embodiments described above. Therefore, any modifications or equivalent substitutions to the present invention, as well as all technical solutions and improvements that do not depart from the spirit and scope of the invention, are covered within the scope of the claims of the present invention.

Claims

1. A path planning method for an agricultural inspection robot used in mulberry orchards, characterized in that, Construct a raster map of the work area; fit a first path in the raster map based on the endpoints of the travel area in the work area, and generate multiple first nodes on the first path; A diffusion function is constructed based on the similarity metric calculated from the distance between the first nodes and the angle difference of the tangent angle. The diffusion function is then used to generate a diffusion coordinate map from the raster map. The A* algorithm, which uses the weighted sum of the Euclidean distance of the raster map and the diffusion distance of the diffusion coordinate map as the heuristic function, plans the global operation path for the travel area. After detecting new obstacles in real time while traveling along the global operation path, a local obstacle avoidance path is planned between the current position and the next first node using the Bi-RRT algorithm that guides path expansion with diffusion distance. The similarity metric calculated based on the difference in distance and tangent angle between the first nodes includes: The similarity measure between all first nodes is obtained by multiplying the Gaussian kernel function of the Euclidean distance between each pair of first nodes with the Gaussian kernel function of the tangent angle. The calculation formula is as follows: ； Where i and j are the indices of the first node; The elements of the similarity matrix W represent the similarity between the i-th and j-th first nodes in the first node sequence. , Let be the diffusion coordinates of the i-th and j-th first nodes; n is the number of first nodes; and This represents the tangent angle of the farm road at the i-th first node and the j-th first node; The variance parameter of the spatial Euclidean distance of all first nodes is used to adjust the similarity weight of the Euclidean distance between first nodes; The variance parameter is the difference in angles between the tangent angles of all first nodes.

2. The path planning method for an agricultural inspection robot in a mulberry orchard according to claim 1, characterized in that, The similarity metric calculated based on the distance and tangent angle difference between the first nodes constructs the diffusion function, including: The similarity metric is calculated based on the distance between the first nodes and the angle difference of the tangent angle, and a similarity matrix is ​​constructed using the similarity metric as elements; The degree matrix is ​​constructed by using the sum of the elements in each row of the similarity matrix as the element values ​​of the corresponding row on the main diagonal of the diagonal matrix; the diffusion matrix is ​​obtained by multiplying the transpose of the degree matrix with the similarity matrix. Perform eigenvalue decomposition on the diffusion matrix, sort it from largest to smallest to obtain multiple eigenvalues ​​and corresponding eigenvectors, and construct the diffusion function based on the eigenvalues ​​and eigenvectors.

3. The path planning method for an agricultural inspection robot in a mulberry orchard according to claim 2, characterized in that, The diffusion function, constructed based on eigenvalues ​​and eigenvectors, is calculated using the following formula: ； in, Let be the diffusion coordinates of the i-th first node; t is the diffusion time parameter, and d is the number of eigenvectors. , The first and dth eigenvalues ​​are raised to the power of t. , Let i be the i-th component of the 1st and dth eigenvectors, where i is the index of the first node.

4. The path planning method for an agricultural inspection robot in a mulberry orchard according to claim 1, characterized in that, The heuristic function is the weighted sum of the Euclidean distance of the raster map and the diffusion distance of the diffusion coordinate map, and the calculation formula is as follows: ； Where f is the weighted sum of Euclidean distance and diffusion distance; h is the Euclidean distance; g is the diffusion distance; and a and b are weighting coefficients.

5. The path planning method for an agricultural inspection robot in a mulberry orchard according to claim 1, characterized in that, The Bi-AM-RRT algorithm for planning local obstacle avoidance paths by guiding path expansion with diffusion distance includes: The Bi-RRT algorithm is used for random tree growth. When searching for tree nodes, the nearest tree node with the smallest diffusion distance to the sampled node is obtained. When expanding a new tree node, if there is no obstacle between the nearest tree node and the sampled node, the sampled node is used as the new tree node. If there is an obstacle between the nearest tree node and the sampled node, the position of the preset step size is intercepted from the nearest tree node towards the sampled node based on the diffusion distance as the new tree node, and the new tree node is connected to the random tree.

6. The path planning method for an agricultural inspection robot in a mulberry orchard according to claim 5, characterized in that, Also includes: When the inspection robot moves to the next path node, the random tree with the current position as the root node is shifted to the next path node accordingly. If the number of root nodes shifted each time is less than or equal to the first node threshold, the neighboring nodes of the new root node are searched and updated. If the number of offset root nodes is greater than the threshold of the first node, the root reconnection optimization is performed based on the Euclidean distance between the previous root node and the target node to update the random tree node relationship.

7. A path planning method for an agricultural inspection robot in a mulberry orchard according to claim 5, characterized in that, After expanding the new tree node, the method further includes: if the cumulative number of new tree nodes expanded each time is greater than the second node threshold, constructing a reconnection ellipse with the new tree node and the target node as the focus and the major axis parallel to the direction of the farm road, triggering target reconnection optimization to search for the tree node with the smallest diffusion distance from the target node in the reconnection ellipse, which is recorded as the target reconnection node; and generating a new path between the target reconnection node and the target node.

8. The path planning method for an agricultural inspection robot in a mulberry orchard according to claim 1, characterized in that, The construction of the raster map includes: Multiple point cloud fitting plane models are obtained from the collected point cloud set. Dynamic thresholds are adaptively calculated based on the slope angle of the plane models. The RANSAC plane fitting algorithm for point clouds is used to filter the point clouds using dynamic thresholds. Ground models are fitted to the point clouds collected in the work area, and the ground models are rasterized into a raster map.

9. A path planning system for an agricultural inspection robot in a mulberry orchard, characterized in that, include: The map module is used to build a raster map of the work area; Based on the endpoints of the travel area in the work area, a first path is fitted in the grid map, and multiple first nodes are generated on the first path; The transformation module is used to construct a diffusion function based on the similarity metric calculated from the distance between the first nodes and the angle difference of the tangent angle, and to generate a diffusion coordinate map for the raster map using the diffusion function; The global planning module is used to plan the global operation path for the travel area using the A* algorithm, which uses the weighted sum of the Euclidean distance of the raster map and the diffusion distance of the diffusion coordinate map as the heuristic function. The local obstacle avoidance module is used to plan a local obstacle avoidance path between the current position and the next first node by using the Bi-RRT algorithm, which guides the path expansion by diffusion distance, after detecting new obstacles in real time while traveling along the global operation path. The similarity metric calculated based on the difference in distance and tangent angle between the first nodes includes: The similarity measure between all first nodes is obtained by multiplying the Gaussian kernel function of the Euclidean distance between each pair of first nodes with the Gaussian kernel function of the tangent angle. The calculation formula is as follows: ； Where i and j are the indices of the first node; The elements of the similarity matrix W represent the similarity between the i-th and j-th first nodes in the first node sequence. , Let be the diffusion coordinates of the i-th and j-th first nodes; n is the number of first nodes; and This represents the tangent angle of the farm road at the i-th first node and the j-th first node; The variance parameter of the spatial Euclidean distance of all first nodes is used to adjust the similarity weight of the Euclidean distance between first nodes; The variance parameter is the difference in angles between the tangent angles of all first nodes.

Images