Agricultural inspection robot path planning method and system for mulberry orchard

Abstract

The invention relates to the technical field of path planning, in particular to an agricultural inspection robot path planning method and system for a mulberry orchard. The method comprises the following steps: constructing a grid map for an operation area; fitting a first path in the grid map based on an end point of a travel area in the work area, and generating a plurality of first nodes on the first path; building a diffusion function based on similarity measurement calculated based on the distance between the first nodes and the angle difference of the tangent angles, and generating a diffusion coordinate map for the grid map by using the diffusion function; a global operation path is planned for the advancing area through an A * algorithm with the weighted sum of the Euclidean distance of the grid map and the diffusion distance of the diffusion coordinate map as a heuristic function; after a new obstacle is detected in real time during advancing along a global operation path, a local obstacle avoidance path is planned between a current position and a next first node through a Bi-AM-RRT algorithm for guiding path expansion through a diffusion distance, and the tracking performance of path planning is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path planning, in particular to a path planning method and system for an agricultural inspection robot in mulberry orchard. BACKGROUND

[0002] To ensure the safety of operation and crop protection in mulberry orchard, the inspection robot needs to have efficient and accurate obstacle avoidance capability. In the prior art, for example, the Chinese patent with the publication number CN112393728A discloses a mobile robot path planning method based on A* algorithm and RRT* algorithm, taking the starting point coordinate and the end point coordinate of the current movement of the mobile robot, and discretizing the line segment connecting the starting point coordinate and the end point coordinate into N points; the A* algorithm and the RRT* algorithm are used to plan the path according to the N points obtained by discretization.

[0003] The existing path planning algorithm is usually based on the assumption of flat ground, and the Euclidean distance calculated in Cartesian coordinates is used as the measurement standard, only considering the relationship of the path nodes in the Cartesian space position. However, in the same machine path, the path nodes with closer spatial positions have higher spatial similarity of adjacent terrains or crops, or are in the same straight or curved part, and the tangent direction consistency of the machine path is higher; at the same time, the advancing direction in the same machine path is usually consistent. The existing path planning algorithm cannot consider the spatial similarity and tangent direction consistency of the path nodes, which makes the path planning deviate from the center line of the machine path or cross the crop row.

[0004] Therefore, there is an urgent need for a path planning method and system for an agricultural inspection robot in mulberry orchard, which can improve the efficiency of path planning while guiding the path to be planned along the direction of the machine path, and improve the tracking performance of path planning. SUMMARY

[0005] The present application aims to solve the technical problem that the Euclidean distance calculated in Cartesian coordinates is used as the core measurement standard in the prior art, which leads to the deviation of path planning from the center line of the machine path, or even the crossing of the crop row. The present application provides a path planning method and system for an agricultural inspection robot in mulberry orchard, which introduces the spatial similarity and tangent direction similarity of the path nodes, and guides the path to be planned along the direction of the center line of the machine path, thereby improving the accuracy and tracking performance of path planning.

[0006] To achieve the above-mentioned application purposes, the present application provides the following technical solutions: A path planning method for an agricultural inspection robot in mulberry orchard, comprising constructing a grid map of the operation area; fitting a first path based on the end points of the advancing area in the operation area in the grid map, and generating a plurality of first nodes on the first path; The similarity metric calculated based on the distance between the first nodes and the angle difference of tangent angles is used to construct a diffusion function, and the diffusion function is used to generate a diffusion coordinate map for the grid map; An A* algorithm with a weighted sum of the Euclidean distance of the grid map and the diffusion distance of the diffusion coordinate map as a heuristic function is used to plan a global work path for the travel area; After detecting a new obstacle in real time during travel along the global work path, a Bi-AM-RRT algorithm that expands the path based on the diffusion distance is used to plan a local obstacle avoidance path between the current position and the next first node.

[0007] The application also provides a path planning system for an agricultural inspection robot in a mulberry garden, comprising: A map module is configured to construct a grid map for a work area, fit a first path in the grid map based on endpoints of a travel area in the work area, and generate a plurality of first nodes on the first path; A conversion module is configured to construct a diffusion function based on a similarity metric calculated based on the distance between the first nodes and the angle difference of tangent angles, and generate a diffusion coordinate map for the grid map using the diffusion function; A global planning module is configured to plan a global work path for the travel area by an A* algorithm with a weighted sum of the Euclidean distance of the grid map and the diffusion distance of the diffusion coordinate map as a heuristic function; A local obstacle avoidance module is configured to plan a local obstacle avoidance path between the current position and the next first node by a Bi-AM-RRT algorithm that expands the path based on the diffusion distance after detecting a new obstacle in real time during travel along the global work path.

[0008] Compared with the prior art, the application has the following beneficial effects: 1. The improved A* algorithm is used to plan a global work path for the mulberry garden to avoid static obstacles in advance, and the improved Bi-AM-RRT algorithm is used to plan a local obstacle avoidance path based on the nodes on the global work path after detecting a dynamically appearing new obstacle, so that the robot can return to the global work path after completing local obstacle avoidance, the local obstacle avoidance path is closer to the global work path, the inspection robot can efficiently work along the pre-planned global work path and avoid unknown obstacles appearing dynamically. Meanwhile, the diffusion coordinate map is generated for the grid map of the mulberry garden in the Cartesian coordinate space based on the spatial similarity and tangent direction similarity of the first nodes of the machine furrow. The diffusion distance calculated based on the diffusion coordinate is introduced into the improved A* algorithm and the Bi-AM-RRT algorithm, so that the planned path nodes have spatial distance optimality, spatial similarity and machine furrow directionality, which is beneficial to improve the accuracy of path planning and the tracking performance along the machine furrow direction.

[0009] 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.

[0010] 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

[0011] Figure 1 A schematic diagram of the structure of an agricultural inspection robot path planning system for mulberry orchards provided in this application; Figure 2 A flowchart illustrating a path planning method for an agricultural inspection robot used in mulberry orchards, as provided in this application; 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

[0012] 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.

[0013] 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.

[0014] 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: a map module configured to construct a grid map for the work area, fit a first path in the grid map based on end points of a travel area in the work area, and generate a plurality of first nodes on the first path; a conversion module configured to construct a diffusion function based on a similarity measure calculated from distances between the first nodes and angle differences of tangent angles, and generate a diffusion coordinate map for the grid map using the diffusion function; a global planning module configured to plan a global work path for the travel area by an A* algorithm with a weighted sum of Euclidean distances of the grid map and diffusion distances of the diffusion coordinate map as a heuristic function; a local obstacle avoidance module configured to plan a local obstacle avoidance path between a current position and a next first node by a Bi-AM-RRT algorithm with diffusion distances guiding path expansion after detecting a new obstacle in real time while traveling along the global work path.

[0015] In another embodiment, the map module further comprises a positioning submodule configured to obtain satellite signals and output centimeter-level positioning information using an RTK-GNSS high-precision positioning technology. A main antenna and a slave antenna of the RTK-GNSS module are respectively installed on two sides of the robot, and an initial lateral distance between the main antenna and the slave antenna is 0.5 m.

[0016] In another embodiment, the local obstacle avoidance module further comprises an environment acquisition submodule configured to obtain an RGB image and a depth image representing a distance between the environment and the robot of the mulberry garden environment by a depth camera, and obtain a point cloud set by a laser radar. The RGB image, the depth image, and the point cloud set are spatially aligned and temporally aligned by a checkerboard calibration board. In this embodiment, the laser radar is a mechanical multi-line laser radar, and an initial installation height is 1.0-1.5 m from the ground to take into account detection of crop canopy and ground obstacles. The scanning frequency is set to 20 Hz. The depth camera is an RGB-D camera, which is installed at the front end of the robot, and the laser radar is installed below the installation plate.

[0017] In another embodiment, a calibration module is further included, which is configured to complete extrinsic calibration of the laser radar and the depth camera by the checkerboard calibration board before constructing the grid map, to ensure that the coordinate spaces of the acquired point cloud set and the depth camera image are aligned; and receive manually marked key positions of the mulberry garden, such as end points of mulch ways and mulberry rows, to complete absolute calibration of the RTK-GNSS.

[0018] For example Figure 2 As shown in FIG. 1, a flowchart of a path planning method for an agricultural inspection robot in a mulberry garden is provided, which comprises the following steps: constructing a grid map for a work area, fitting a first path in the grid map based on end points of a travel area in the work area, and generating a plurality of first nodes on the first path; A similarity measure is calculated based on the distance between the first nodes and the angle difference of the tangent angles, and a diffusion function is constructed based on the similarity measure, and the diffusion coordinate map is generated using the diffusion function. The global work path is planned by A* algorithm using 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 during the travel along the global work path, the local obstacle avoidance path is planned by Bi-AM-RRT algorithm between the current position and the next first node by guiding the path expansion with the diffusion distance.

[0019] Specifically, in the embodiment, the work area corresponds to a mulberry orchard, which includes a travel area corresponding to a machine path between mulberry tree rows and an obstacle area corresponding to an obstacle area where obstacles exist, wherein the obstacles include but are not limited to mulberry tree rows, fixedly arranged work equipment, and monitoring equipment. The grid map corresponds to the spatial environment modeling of the mulberry orchard. In the embodiment, the RTK-GNSS positioning technology is used to collect key positions of the work area to establish boundary constraints, such as the endpoints of the machine path and the mulberry tree rows; wherein the key positions are identified by the work personnel, at least including the corner points of the work area, the endpoints of the machine path, and the corner position points of the obstacles, and the grid map is generated based on these key positions in the work area by the RTK-GNSS tool library, which is the first grid map. In the first grid map, the grid is labeled with the machine path for the inspection robot to travel, the mulberry tree rows representing the mulberry tree planting area, and other fixed obstacles, and the start and end points of each machine path are marked according to the preset work travel direction. It should be noted that when the machine path and the mulberry tree rows in the mulberry orchard remain unchanged, the global map planned for the first work of the mulberry orchard can be saved in the database, and the global map can be directly obtained from the database in subsequent work of the same mulberry orchard.

[0020] Further, in the first grid map, the midpoint of the two ends of the machine path is taken as the start and end points, and the first path is fitted by a straight line fitting function to represent the straight line reference direction of the machine path, so as to reflect the overall trend of the machine path. Between the start and end points of the first path, the first path is discretized by identifying a node every first interval, to generate a plurality of first nodes. In the embodiment, the first interval is set to 5m. The work personnel identify the key positions such as the endpoints of the machine path, the inflection points of the machine path, the crossing points of the crop rows, and the position points of the obstacles on the first path, and also mark them as first nodes. The plurality of first nodes are added to an array in the order of the preset travel direction to form a first node sequence.

[0021] Further, taking the current position of the robot as a starting node, and the first node not participating in planning with the minimum Euclidean distance from the current position as a target node, the A* algorithm is used to plan the global work path of the machine path. The A* algorithm takes the actual cost and the estimated cost as the heuristic function to calculate the total cost between the current position of the inspection robot and the coordinates of the target node, wherein the actual cost is the Euclidean distance between the starting point of the current path, i.e. the i-th first node in the first node sequence, and the current position of the inspection robot; the estimated cost is the Euclidean distance between the current position of the inspection robot and the target node, i.e. the i+1-th first node; the new path node is expanded between the current position and the target node according to the total cost, which guides the path search to converge to the target node, and finally plans the shortest path from the starting point to the end point of the machine path. However, in the A* algorithm, the calculation of the Euclidean distance only considers the spatial shortest path, and by default, all adjacent feasible nodes are expanded. If the travel direction of the first path is not integrated when the new path node is expanded, the cumulative expansion of multiple times may cause the global work path to deviate from the center line of the machine path more, and thus the path planning efficiency between subsequent nodes is reduced.

[0022] Since the mulberry garden usually includes multiple machine paths, the inspection robot travels in a back-and-forth manner on the multiple machine paths, i.e. the endpoint of one machine path serves as the starting point of the adjacent machine path, so the travel directions of the adjacent machine paths are usually opposite. At the same time, the machine paths in the actual mulberry garden also have curved parts, so that there is a tangent angle between the straight line between the curved part of the machine path and the endpoint of the machine path. In the environment space, the closer the distance between the first nodes, the higher the spatial similarity between them, for example, there may be the same mulberry tree or the same piece of soil between adjacent first nodes; the smaller the angle difference of the tangent angle between the first nodes, the more consistent the tangent directions of the two first nodes. Based on the spatial similarity and the tangent direction similarity of the first nodes, as a similarity measure between the first nodes, it is beneficial to guide the global work path to follow the first path in path planning.

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

[0024] Further, when the A* algorithm is executed, the Euclidean distance between the current position of the inspection robot and the Cartesian coordinates of the target node is calculated by the Euclidean distance formula, and the diffusion distance of the diffusion coordinates is calculated; the heuristic function is set as the weighted sum of the Euclidean distance between the current position of the inspection robot and the target node and the diffusion distance, so that the path planning is more inclined to expand the path along the direction of the farm road, and the tracing property of the global work path along the direction of the farm road is improved, and the calculation formula is as follows: ; wherein f is the weighted sum of the Euclidean distance and the diffusion distance; h is the Euclidean distance between the current position and the Cartesian coordinates of the target node; g is the diffusion distance between the current position and the diffusion coordinates of the target node; a and b are weight coefficients, and in the embodiment, the weight ratio of a and b is set to 7:3. The A* algorithm with improved heuristic function is used to plan the path between the adjacent first nodes in the first node sequence along the advancing direction, and the planned path is smoothed by using cubic spline interpolation to obtain the global work path along the farm road.

[0025] When the inspection robot works, it first advances along the statically planned global work path. During the advancing of the inspection robot, the change rate of the steering angle of the tracked chassis is ensured to be less than or equal to 10 degrees per second, so as to reduce the situation of rapid change of the steering angle during the advancing. Since there are new dynamic obstacles such as workers or other work robots in the mulberry orchard, the obstacles need to be detected in real time during the advancing to trigger the local path planning to complete the obstacle avoidance. In the embodiment, the moving speed and the distance from the current position of the obstacle are detected in real time by the laser radar ranging mode to determine whether the obstacle is a static obstacle or a dynamic obstacle. The dynamic obstacle is regarded as a new obstacle; for the static obstacle, it is determined whether the corresponding grid of the static obstacle in the first grid map has an obstacle mark. If not, it indicates that it is a newly added static obstacle, and the local path planning also needs to be performed.

[0026] Further, in another embodiment, since it is difficult to manually identify the obstacles in the real-time acquired environment image and determine whether the obstacles are moving during the work. In the embodiment, further comprising: scanning by the laser radar at 20Hz to acquire the point cloud set in front of the inspection robot in real time, and determining whether the obstacle is a static obstacle or a dynamic obstacle by the size and moving speed of the dense point cloud part; acquiring the RGB image and the depth image in front of the inspection robot by the depth camera, identifying the obstacle category such as static crop row and dynamic worker from the environment image by using the YOLOv8 model, and then marking the grid with obstacles in the grid map.

[0027] Specifically, when the inspection robot works along the global working path, the point cloud set is input into the DBSCAN algorithm, and the neighborhood radius, the minimum number of points and the maximum number of iterations are set; for the first point cloud in the traversal, if the neighborhood of the first point cloud contains at least the minimum number of other point clouds, the first point cloud is identified as a core point, and the core point and its reachable points are clustered into a point cloud block based on density reachability. In this embodiment, the size of the point cloud block includes the horizontal width and the vertical height measured along the direction perpendicular to the track, and in other embodiments, it can also include the point cloud block thickness along the direction parallel to the track calculated based on the depth map. The point cloud block with a vertical height greater than 30 centimeters and a horizontal width greater than 20 centimeters is identified as an obstacle. In an embodiment, it further includes: when the laser radar detects a new obstacle, the distance between the obstacle and the inspection robot is calculated by laser ranging technology and the relative speed; if the distance is less than 5 meters and the relative speed is greater than 0, the obstacle is identified as a dynamic obstacle; if the distance is less than 3 meters and the relative speed is equal to 0, the obstacle is identified as a static obstacle.

[0028] In this embodiment, the Bi-AM-RRT algorithm is a Bi-RRT (Bidirectional Rapidly-Exploring Random Tree) algorithm guided by diffusion distance for path expansion. The Bi-AM-RRT algorithm is used to perform local path planning to obtain a local obstacle avoidance path. The Bi-RRT (Bidirectional Rapidly-Exploring Random Tree) algorithm selects the nearest neighbor node in the tree based on the Euclidean distance calculated in the Cartesian coordinate space, and expands a new tree node. However, the local obstacle avoidance path planned by the Bi-RRT algorithm may not be the shortest path. In order to make the planned local obstacle avoidance path also tend to follow the first path, an improved Bi-AM-RRT algorithm is adopted, taking the current position of the inspection robot as the starting node, and the first node behind the obstacle in the travel direction and having the minimum Euclidean distance from the current position as the target node, and performing double-tree expansion at the starting node and the target node. When selecting the nearest neighbor node and expanding the new tree node, diffusion coordinates representing the spatial similarity between tree nodes and the similarity of track tangent directions are used, and the diffusion distance is calculated by the Euclidean distance formula to guide the obstacle avoidance path to also tend to follow the first path, thereby improving the tracking performance of the local obstacle avoidance path along the track direction.

[0029] The inspection robot first travels along the global working path, and when it identifies in real time that there is an obstacle in the mulberry orchard, it bypasses the obstacle along the local obstacle avoidance path, and then returns to the global working path. When the RTK-GNSS positioning of the inspection robot reaches the last first node in the first node sequence, and the positioning accuracy is less than or equal to 0.5 m, and the ratio of the number of connections between the global planning path and the local obstacle avoidance path to the total number of first nodes is greater than or equal to 95%, the sub-working task is completed.

[0030] The improved A* algorithm is used for planning the global operation path of mulberry garden to avoid static obstacles in advance. After detecting the new dynamic obstacles, the improved Bi-AM-RRT algorithm is used for planning the local obstacle avoidance path, and the global operation path is returned after completing the local obstacle avoidance, so that the inspection robot can not only work efficiently along the global operation path planned in advance, but also avoid unknown dynamic obstacles. Meanwhile, based on the spatial similarity and tangent direction similarity of the first node of the machine path, the diffusion coordinate map is generated for the mulberry garden in the Cartesian coordinate space. The diffusion distance of the diffusion coordinate calculation is introduced into the improved A* algorithm and Bi-AM-RRT algorithm, so that the planned path node has spatial distance optimality, spatial similarity and machine path same direction, which is beneficial to improve the accuracy of path planning and the tracking along the machine path direction.

[0031] In the process of global path planning and local path planning, the Euclidean distance of spatial distance calculated by Cartesian coordinates is used to search for the nearest node; the diffusion distance representing the spatial similarity and tangent direction similarity between nodes is calculated by introducing the diffusion coordinate calculation, which is beneficial to search for the nodes in the same machine path to guide the path to expand along the machine path direction, and is beneficial to improve the tracking during path planning.

[0032] In an embodiment of the present application, a diffusion function is constructed based on the similarity measure calculated by the distance and the angle difference of the tangent angle between the first nodes, for example Figure 3 As shown in the figure, it comprises: The similarity measure is calculated based on the distance and the angle difference of the tangent angle between the first nodes, and a similarity matrix is constructed with the similarity measure as the element; The sum of each row element of the similarity matrix is used as the element value of the corresponding row on the main diagonal of the diagonal matrix to construct a degree matrix; the transpose matrix of the degree matrix is multiplied by the similarity matrix to obtain a diffusion matrix; The diffusion matrix is subjected to eigenvalue decomposition, and a plurality of eigenvalues and corresponding eigenvectors are obtained by sorting from large to small, and a diffusion function is constructed based on the eigenvalues and eigenvectors.

[0033] Specifically, the similarity measure between the first nodes is calculated, and a similarity matrix W is constructed based on the similarity measure between the first nodes. The first node sequence is obtained, the tangent direction of the machine path and the tangent angle between the straight line of the machine path endpoint are calculated; the Euclidean distance between all first nodes is calculated using Cartesian coordinates, which is used to represent the similarity of the first nodes in distance; the angle difference of the tangent angle between all first nodes is calculated, which is in radians (rad), and is used to represent the similarity of the first nodes in the tangent direction of the machine path, so that the first nodes with the same direction are preferentially connected during path planning; the product of the Gaussian kernel function of the distance and the Gaussian kernel function of the tangent angle between all first nodes is calculated, to obtain the similarity measure between the first nodes, and 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, used to adjust the similarity weights for the differences in tangent directions between first nodes.

[0034] 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: ; Transpose the degree matrix Multiplying the similarity matrix W by the diffusion matrix P yields the diffusion matrix P, calculated using the following formula: ; 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: ; 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. , is the i-th component of the 1st, d-th feature vector; i is the first node index; in the embodiment, d is set to 2 for planar coordinates and set to 3 for three-dimensional coordinates. The coordinates of all grids in the first grid map are mapped to corresponding diffusion coordinates using a diffusion function to obtain a diffusion coordinate map corresponding to the first grid map. The feature vector is used to represent the spatial similarity and tangent direction similarity of the machine path; the feature value represents the similarity strength size, the larger the feature value, the greater the spatial similarity and tangent direction similarity, and the higher the efficiency of guiding the inspection robot to travel along the machine path.

[0035] In an optional embodiment, after mapping the diffusion coordinate map, the diffusion distance between any two first nodes is calculated, and the diffusion distance is stored in a set such as a lookup table or an array, so that it can be directly obtained when subsequent global path planning or local path planning is performed.

[0036] Nodes that are spatially close and have the same tangent direction of the machine path have high similarity. The spatial similarity of the nodes is quantified by the distance between the nodes in the strawberry field, the similarity of the tangent direction of the machine path is quantified by the angle difference of the tangent angle, the similarity measure between the nodes is calculated based on the spatial similarity and the tangent direction similarity, and the diffusion coordinate space is generated for the Cartesian coordinate space of the first grid map based on the diffusion function constructed based on the similarity measure, which is equivalent to compressing the distance perpendicular to the direction of the machine path, so that the probability of the nearest tree node and the sampling node being located on the same or adjacent machine path is increased.

[0037] In an embodiment of the present application, a Bi-AM-RRT algorithm is used to plan a local obstacle avoidance path by guiding path expansion with diffusion distance, including: The Bi-RRT algorithm is used for random tree growth. When searching for a tree node, the nearest tree node with the minimum diffusion distance from the sampling node is obtained. When expanding a new tree node, if there is no obstacle between the nearest tree node and the sampling node, the sampling node is used as the new tree node. If there is an obstacle between the nearest tree node and the sampling node, a position with a preset step length from the nearest tree node towards the sampling node is taken as the new tree node based on the diffusion distance, and the new tree node is connected to the random tree.

[0038] Specifically, the Bi-AM-RRT algorithm is implemented by using the Python tool library in the embodiment. The RTK positioning coordinates of the current position of the inspection robot are set as the starting node, and the first node with the minimum Euclidean distance from the current position is set as the target node; the local grid map with a preset area of 10 meters*10 meters and a resolution of 0.1 meters centered on the current position of the inspection robot is set as the local grid map in the first grid map. The sample nodes are generated by the Sample_state function, and the Euclidean distance between the sample nodes and the tree nodes is greater than or equal to 0.5 meters. If the proportion of the projection area of the obstacle in the local grid map to the area of the local grid map is greater than 0.8, the first node with the minimum Euclidean distance from the current position is directly selected as the sample node; if the proportion of the projection area of the obstacle in the local grid map to the area of the local grid map is less than or equal to 0.8, the side of the obstacle with a larger vertical distance from the boundary of the track and a vertical distance greater than the width of the track of the inspection robot, and the track region with an angle less than 20 degrees from the track in the direction of the inspection robot are selected as the obstacle avoidance region.

[0039] The random tree includes a forward tree based on the current position as the root node, and a reverse tree based on the first node with the minimum Euclidean distance from the current position as the root node. The forward tree and the reverse tree alternately expand new tree nodes in opposite directions. It should be noted that the inspection robot is usually difficult to move directly transversely, and therefore, when expanding the tree nodes, it is necessary to generate a circular arc path adapted to the turning constraint of the inspection robot.

[0040] The sample nodes are randomly obtained in the obstacle avoidance region, and the Nearest(T, x_rand) function is used to search for the nearest tree node with the minimum diffusion distance from the sample node in the current tree, where T is the current tree, and x_rand is the sample node. It is determined whether there is an obstacle mark in the grid corresponding to the connecting line between the sample node and the nearest tree node, and if there is no obstacle between the sample node and the nearest tree node and the track turning constraint is met, the sample node and the nearest tree node are directly connected. The track turning constraint formula is as follows: ; wherein R is the turning radius; L is the track wheelbase; is the turning angle, and the maximum value is 30°. Wherein the turning angle is the angle between the direction of the inspection robot and the straight line of the track detected by the Hough transform.

[0041] If there is an obstacle between the sampling node and the nearest tree node, the moving step length is cut off based on the diffusion distance between the two points in the Steer(x_nearest, x_rand) function from the nearest tree node to the direction of the sampling node, the position of the moving step length from the nearest tree node is the new tree node, wherein x_nearest is the nearest tree node; the steering angle of the nearest tree node and the new tree node is obtained, the difference value of the steering angle is calculated, and the difference value of the steering angle is substituted into the track steering constraint formula to calculate the turning radius. If the difference value of the steering angle is greater than 30 o , or the turning radius is less than the minimum turning radius, the moving step length is increased, and the new tree node is regenerated; if the difference value of the steering angle is less than or equal to 30 o , and the turning radius is greater than or equal to the minimum turning radius, a circular arc path is generated between the two tree nodes through the turning radius, the length between the nearest tree node and the new tree node as the chord length, and the diffusion coordinates of the two tree nodes, and the nearest tree node and the new tree node are connected through the circular arc path. In this embodiment, the minimum moving step length is set to 0.5 meters due to the terrain adaptability of the tracked chassis, so as to reduce the situation that the tracked slip is caused by frequent small amplitude movement.

[0042] After the forward tree or the reverse tree connects the new tree node each time, the Euclidean distance between the new tree node and the tree node in the other tree is calculated, if there is a minimum Euclidean distance less than or equal to the double-tree connection threshold, and the path formed after the tree node corresponding to the minimum Euclidean distance is connected is free of obstacles, and the difference value of the steering angle of the corresponding tree node is less than 15 o , the tree nodes of the reverse tree are integrated into the forward tree in reverse order, and the forward tree and the reverse tree are combined to obtain a local obstacle avoidance path. In this embodiment, the double-tree connection threshold is set to 2.5 times the width of the tracked chassis, so that the path after the double-tree combination has passability.

[0043] In the local path planning, the diffusion distance search tree node and the extended tree node are introduced in the Bi-RRT algorithm to generate a new Bi-AM-RRT algorithm, so that the sampling node and the nearest tree node in the random search have spatial similarity and tangent direction similarity; based on the diffusion distance between the nearest tree node and the sampling node, a new tree node is generated, which tends to extend along the direction of the track where the nearest tree node is located, which is beneficial to improve the tracking performance of the local obstacle avoidance path.

[0044] In an embodiment of the present application, it further includes: when the inspection robot moves to the next path node, the random tree with the current position as the root node is correspondingly shifted to the next path node, if the number of root nodes of each cumulative offset is less than or equal to the first node threshold, the adjacent nodes of the new root node are searched and updated; if the number of root nodes of the offset is greater than the first node threshold, the root reconnection optimization is updated based on the Euclidean distance between the last root node and the target node to update the relationship of the random tree nodes.

[0045] Specifically, in this example, the first node threshold is preset as 2. A root queue is created for temporarily saving the offset root nodes. The local path planning is a dynamic process. While the inspection robot moves along the local obstacle avoidance path, the next path node is also planned until the obstacle avoidance is completed and the global work path is returned. After the inspection robot moves along the local obstacle avoidance path to the next path node, the root node of the random tree is offset to the next path node, i.e., the current position of the inspection robot changes, and the node relationship of the random tree needs to be rebuilt. Therefore, after the inspection robot is offset to the new root node, the neighbor node list of the original root node is emptied, 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, the new root node is taken as the center, and within the preset neighborhood radius, the neighbor tree nodes in the random tree that have a Euclidean distance less than a distance threshold from the new root node, and that are in the direction of the machine-tillage path and have a turning angle less than or equal to 20 o If the number of root nodes accumulated in the root queue each time is greater than 2, the root reconnection optimization is triggered, which means that the root reconnection optimization is performed every two root nodes. The random tree based on the new root node is obtained, and the node relationship of the random tree is updated to reduce the overall path cost, including: dequeuing two root nodes (i.e., the last root node and the root node , ), from the root queue, searching for a neighbor node in the neighbor node list of the root node that has a minimum Euclidean distance from the target node and satisfies the track turning constraint, setting the neighbor node as the parent node of the target node, and updating the node relationship of the random tree by using the Update_edge function, so that the new path adapts to the position offset of the inspection robot.

[0046] In an embodiment of the present application, after the new tree node is expanded in the random tree, the method further includes: presetting a second node threshold, and if the number of new root nodes expanded each time is greater than the second node threshold, taking the new tree node and the target node as the foci, constructing a reconnection ellipse with the long axis parallel to the direction of the machine-tillage path, triggering the target reconnection optimization to search for a tree node having a minimum diffusion distance from the target node in the reconnection ellipse, and recording the tree node as a target reconnection node; and generating a new path between the target reconnection node and the target node.

[0047] Specifically, in this example, the second node threshold is preset to 2. The target reconnection optimization is usually applied to the forward tree, including: when a first node is expanded in the forward tree as a new tree node, the first node is added to the target stack. After a new tree node is generated in the forward tree, target reconnection optimization needs to be performed once every two new tree nodes are expanded. If the number of first nodes in the target stack is greater than 2, two first nodes are popped out; taking the new tree node and the target node as the focus, a reconnection elliptical region is constructed with the long axis parallel to the direction of the machine furrow to promote the generation of a new path along the direction of the machine furrow and improve the alignment degree of the path direction and the direction of the machine furrow. The tree node with the minimum diffusion distance from the target node in the forward tree is searched in the reconnection ellipse, which is recorded as the target reconnection node, and a new path is generated between the target reconnection 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, the branch is emptied to avoid invalid exploration; wherein 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 the target node through other paths.

[0048] In some embodiments, the maximum value of the root reconnection time and the target reconnection time is set to 0.1 seconds, and if the reconnection time is greater than 0.1 seconds, the reconnection optimization is ended to match the steering response speed of the tracked chassis.

[0049] In the mulberry field scene, the ground is the largest continuous surface. The ground usually has ups and downs, so that the height of the collected point cloud set has differences, and the actual path of the inspection robot has ups and downs in the spatial vertical direction. The first grid map used for planning the path constructed by the RTK-GNSS is usually a continuous plane, and it is difficult to reflect the ground slope change, so the global work path and the local obstacle avoidance path planned in the first grid map will be affected by the ground ups and downs, resulting in that the actual path and the planned path are prone to have a large path deviation during the travel. Therefore, in an embodiment of the present application, the grid map is constructed, including: A plurality of point cloud fitting plane models are obtained from the collected point cloud set, a dynamic threshold is adaptively calculated according to the slope angle of the plane model, and a RANSAC plane fitting algorithm using the dynamic threshold is used to fit a ground model to the point cloud set collected in the work area. The ground model is rasterized into a grid map.

[0050] Specifically, first, a plane model in a three-dimensional space is defined as follows: ; Wherein, (A, B, C) is the normal vector of the plane model, D is a constant, representing the distance from the origin to the plane model. Randomly extract 3 points from the point cloud set to form the minimum sample set, and substitute it into the plane model formula to obtain the candidate plane model. The slope angle is calculated by the angle between the normal vector u = (A, B, C) and the vertical vector v = (0, 0, 1) : ; The classic RANSAC plane fitting algorithm uses a fixed threshold to filter the point cloud, and the point cloud on the slope far from the current position of the inspection robot has a larger vertical distance relative to the improper plane model, so the point cloud far away is easy to be misjudged as a non-ground point. Therefore, in this embodiment, an adaptive dynamic threshold T is determined according to the slope angle: ; Wherein, is the basic threshold, set to 0.1 meters; k is the slope coefficient, set to 0.1; ω is the slope angle, expressed in radians. Calculate the distance of all point clouds to the plane model, and mark the point cloud with a distance less than the dynamic threshold T as a candidate point cloud. In order to avoid the dynamic threshold being too large under an extremely steep slope, the upper limit T max of the dynamic threshold is set to 1 meter.

[0051] Repeat the iteration multiple times, select the plane model with the most candidate point clouds after fitting multiple plane models, and select the corresponding candidate point cloud as the ground point cloud. Use a statistical filtering algorithm to remove outliers from the ground point cloud, set the resolution to 0.1 meters, and rasterize the ground model to obtain a grid map. Wherein, the iteration number N: ; Wherein, p is the success probability of the algorithm; w is the number ratio of the candidate point cloud to the point cloud set; p and w are set according to the historical fitting results; m is the minimum sample size required to construct a plane model, which is set to 3 in this embodiment; w m represents the probability that m randomly selected point clouds are all candidate point clouds.

[0052] In the RANSAC plane fitting algorithm, a dynamic threshold for screening point clouds is adaptively determined based on the slope angle. The dynamic threshold is proportional to the slope angle of the currently fitted plane model. The steeper the slope, the larger the dynamic threshold, so that the probability of the point cloud on the steep slope being screened as the ground point cloud is greater, so that the final fitted ground model can reflect the ground undulation difference to a certain extent. Path planning on this ground model is beneficial to improve the accuracy of path planning.

[0053] The above examples are only used to illustrate the technical solutions described in the present application and do not limit the present application. Although the present application has been described in detail with reference to the above examples, the present application is not limited to the above specific embodiments. Therefore, any modification or equivalent replacement of the present application; and all technical solutions and improvements without departing from the spirit and scope of the application are all included in the scope of the claims of the present application.

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-AM-RRT algorithm, which guides the path expansion by diffusion distance.

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; 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. 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 similarity metric calculated based on the distance and tangent angle difference between the first nodes includes: Calculate the product of the Gaussian kernel function of the distance between all first nodes and the Gaussian kernel function of the tangent angle, using the following formula: ； Where i and j are the indices of the first node; These are the elements of the similarity matrix W; , 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 between the i-th and j-th first nodes; is the variance parameter of the Euclidean distance; This is the variance parameter of the angle difference between tangent angles.

4. 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.

5. 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.

6. 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.

7. A path planning method for an agricultural inspection robot in a mulberry orchard according to claim 6, 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.

8. A path planning method for an agricultural inspection robot in a mulberry orchard according to claim 6, 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.

9. A 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.

10. 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-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.

Images