Orchard robot path planning method based on ground undulation degree self-adaption

Abstract

The invention belongs to the field of intelligent agriculture, and particularly discloses an orchard robot path planning method based on ground undulation degree self-adaption, and the method has the capability of dynamically adjusting an advancing path according to the ground undulation degree, and comprises the steps: generating a ground undulation degree map according to pre-mapped ground data, according to the method, the moving task of the orchard robot from the starting point to the target point is designated, path planning is conducted through the A * algorithm based on the ground undulation degree and the obstacle detection method, an initial path track is obtained, then the path is smoother through path redundancy removal, and the orchard robot can efficiently and safely run in the complex agricultural production environment.

Description

Technical Field

[0001] This article belongs to the field of smart agriculture, mainly involving agricultural robot production and path planning methods for complex road sections.

[0002] This project is a key project of the Jiangsu Provincial Key R&D Program (Industry Foresight and Key Core Technologies) (Project No.: BE2021016). Background Art

[0003] With the continuous advancement of science and technology, modern intelligent agricultural production has gradually become the key to achieving high efficiency and low cost. Orchard robots are also gradually entering the daily production environment and becoming a powerful assistant to fruit farmers. The domestic orchard planting area is large, and the output and demand are huge. However, the intelligence level of orchard robots still needs to be further improved. Considering that the main tasks of orchard robots include picking, pesticide removal, and garden inspections, and orchards are often located on rugged terrain such as hills and mountains, the obstacles in the environment are irregular in shape and change frequently, and are also affected by various weather factors. This puts high demands on the accuracy and real-time performance of the orchard robot's path planning. How to optimize the path planning of the orchard picking robot is the key to improving its autonomous navigation performance.

[0004] Nowadays, most path optimization focuses on how to find the shortest path in space and improve the efficiency of the algorithm. For example, in "Path Planning of a Quadruped Inspection Robot for Gas Micro-Leakage Based on an Improved A* Algorithm", an adaptive heuristic function is proposed to optimize the path based on the ratio of the Manhattan distance between the current node position and the end point and the Manhattan distance from the starting point to the end point. Patent CN202411044300.X uses an extended search A* algorithm to search in a forward three-candidate point extended search manner, and uses a weighted cost function to calculate the weighted cost from the candidate point to the three forward candidate points. The candidate point with the smallest weighted cost is selected and added to the closed list, thereby improving the algorithm efficiency and application scope. Patent CN202410934049.8 uses an improved A* algorithm to perform global obstacle avoidance path planning on the prior obstacle grid map, eliminates redundant path sub-target points in the global obstacle avoidance path, and uses an improved dynamic window method to perform local obstacle avoidance planning on the global obstacle avoidance path that eliminates redundant path sub-target points.

[0005] However, in the actual production environment, the shortest path in space does not mean the most efficient, because the road surface in the actual environment is uneven, which will have a great impact on the operation of the robot. Every time the robot passes a slope, it will take longer to climb the slope than to drive on flat ground, and its driving path will be offset. The faster the robot drives, the greater the offset angle. If this offset is not corrected, it will accumulate, and eventually the actual operation route will deviate greatly from the planned route. In more extreme cases, the robot will roll over when passing through these areas with high ground undulations quickly. This situation is unacceptable in the actual production environment. Therefore, in actual operation, the shortest path can be sacrificed temporarily in exchange for longer-term and stable operation of the robot, and the robot can drive faster and more stably on flat ground. At this time, although the route obtained under path planning is not the shortest route at the spatial level, it is the optimal path at the temporal level. Summary of the Invention

[0006] In order to solve the route planning problem of orchard robots in uneven ground environments, this paper proposes an orchard robot path planning method based on ground undulation adaptation. The specific scheme is as follows:

[0007] A path planning method for an orchard robot based on ground undulation adaptation comprises the following steps:

[0008] S1: Generate the corresponding ground relief map based on the pre-surveyed ground data;

[0009] S2: Select the starting point and end point of the robot;

[0010] S3: Use the A* algorithm based on ground relief and obstacle detection method to perform path planning and obtain the initial path;

[0011] S4: Remove redundant nodes from the generated initial path to make the path smoother and finally obtain the optimized path.

[0012] In step S3, the actual working environment of the orchard robot is often located in rugged areas. When the robot passes through these areas, the wheels will deviate. The offset will increase with the increase of the ground undulation, and these offsets will accumulate. When the accumulation reaches a certain level, the error between the actual driving route and the planned route of the robot will be very large. Therefore, a new path planning method is proposed, which uses the A* algorithm based on the ground undulation. The evaluation function of this algorithm is:

[0013] F(n)=G(n)+k(n)·H(n)

[0014]

[0015] Where G(n) is the actual cost, which is the actual cost from the starting node to the pre-selected node;

[0016] H(n)——estimated cost;

[0017] k(n) — weight function;

[0018] α n ——ground relief at the nth node;

[0019] α k ——the average value of ground relief at all nodes;

[0020] b——weight proportional coefficient.

[0021] Nodes with a ground relief of 0° are considered flat, nodes with a ground relief greater than 0° and less than 15° are considered low-ground relief nodes, and nodes with a ground relief greater than or equal to 15° are considered high-ground relief nodes. The value of the b weight proportional coefficient is determined based on the ratio of the number of nodes with high ground relief to the number of nodes with low ground relief. If the value of the weight function is larger, the algorithm will place more emphasis on estimating the cost, and therefore will give priority to avoiding these nodes with high ground relief.

[0022] In step S3, the A* algorithm treats the robot as a point mass in path planning, without considering its size. As a result, the robot will be very close to obstacles during actual operation. To solve this problem, this paper proposes an obstacle avoidance method. The specific steps are as follows:

[0023] S3.1: Set a warning buffer zone with a width of two grids near the obstacle;

[0024] S3.2: Place the eight test nodes around the current node into the sequence. If the line connecting these test nodes to the original node encounters an obstacle, delete this node from the sequence.

[0025] S3.3: Repeat the S3.2 operation for the filtered nodes, and then execute the remaining procedures of the A* algorithm based on ground relief for the nodes to be tested that have not been deleted until there are no more nodes to be tested.

[0026] The optimal path searched by the commonly used path planning algorithm for grid maps in step S4 often has many turning points, and there are also problems such as uneven paths and large broken line angles. To solve this problem, this paper proposes to delete some redundant nodes on the original planned route. The specific steps are as follows:

[0027] S4.1: First determine the starting point of the original route planning, and then connect the subsequent nodes in sequence;

[0028] S4.2: Determine whether the line connecting the two nodes touches an obstacle and whether it is outside the safe distance. If not, treat the current node as a redundant point. Otherwise, add the previous node of the secondary node to the modified path node set.

[0029] S4.3: Repeat steps S4.2 until you reach the end.

[0030] Beneficial effects:

[0031] A ground undulation map is generated based on pre-surveyed ground data, and the orchard robot's movement task from a starting point to a target point is specified. Path planning is performed using an A* algorithm based on ground undulation and an obstacle detection method to obtain an initial path trajectory. The path is then smoothed by removing redundancy. This invention enables the orchard robot to travel efficiently and safely in complex agricultural production environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 This is an A* path planning flow chart of an orchard robot path planning method based on adaptive ground relief;

[0033] Figure 2 It is a ground relief map for an orchard robot path planning method based on ground relief adaptation;

[0034] Figure 3 This is an interface diagram for users to select the starting point and end point of an orchard robot path planning method based on adaptive ground relief;

[0035] Figure 4 It is an obstacle detection map of nodes to be measured for an orchard robot path planning method based on adaptive ground relief;

[0036] Figure 5 This is a schematic diagram of removing redundant nodes from the initial path of an orchard robot path planning method based on adaptive ground relief;

[0037] Figure 6 This is a path planning diagram of the first test of the A* algorithm for orchard robot path planning based on ground undulation adaptation;

[0038] Figure 7 This is a path planning diagram of the second test of the A* algorithm for orchard robot path planning based on ground undulation adaptation; DETAILED DESCRIPTION

[0039] In order to deepen the understanding of the present invention, the present invention will be further described in detail below with reference to the embodiments and drawings. The embodiments are only used to explain the present invention and do not constitute a limitation on the scope of protection of the present invention.

[0040] The present invention will be further described below with reference to the accompanying drawings and specific embodiments:

[0041] like Figure 1 As shown, the method of the present invention comprises the following steps:

[0042] S1: Generate a corresponding ground relief map based on the ground slope and obstacle position information measured in advance by the tilt sensor;

[0043] Use the tilt sensor to measure the slope of a 1m*1m area of ​​land as a node, and record the location information of obstacles. Fill the slope information of each node into a matrix. If the node is an obstacle, set the value to 50. Then generate the following matrix based on this matrix: Figure 2 The land relief map of Figure 2 As shown in the figure, the orchard is divided into a 30*30 grid map, which contains obstacles and high and low ground relief nodes. Obstacles occupy 186 grids (yellow in the figure), high ground nodes occupy 89 grids (bright blue in the figure), low ground nodes occupy 39 grids (dark blue in the figure), and flat land occupies 586 grids (dark blue in the figure).

[0044] S2: Select the robot’s starting point and end point on the generated map, such as Figure 3 As shown;

[0045] S3: Use the A* algorithm based on ground relief and obstacle detection method to perform path planning and obtain the initial path;

[0046] S3.1: The A* algorithm based on ground undulation is a special improvement to the evaluation function of the A* algorithm for the uneven environment of the orchard. The improved evaluation function is:

[0047] F(n)=G(n)+k(n)·H(n)

[0048]

[0049] Where G(n) is the actual cost, which is the actual cost from the starting node to the pre-selected node;

[0050] H(n)——estimated cost;

[0051] k(n) — weight function;

[0052] α n ——ground relief at the nth node;

[0053] α k ——the average value of ground relief at all nodes;

[0054] b——weight ratio coefficient;

[0055] The value of b is the ratio of the number of nodes with low ground relief to the number of nodes with low ground relief. Therefore, in this test, the value is 2.28. In this example, α k is 2.14, such as Figure 6 As shown, we start from (3,21) in the figure. Since the node area is 1m*1m, x1 is 3 and y1 is 21. Then we evaluate the points around the starting point. For example, (4,21) in the figure is flat land, its x2 is 4, y2 is 21, H(1) is 1, its undulation α1 is 0, k(1) is 1, G(1) of this node is 1, H(1) is 1, k(1) is 1, and the F(1) evaluation function value is 2. In the figure, (4,22) is a node with high ground undulation, its x2 is 4, y2 is 2 2, H(1) is 1.4, its fluctuation α1 is 15, k(1) at this point is 8, G(1) at this node is 1.4, H(1) is 1.4, k(1) is 16.98, then the F(1) evaluation function value is 25.17, and the evaluation function values ​​of other nodes around the starting point are deduced in the same way. Then these nodes are input into the obstacle detection, and after being eliminated, the node with the lowest evaluation function value is selected as the next node to travel, and then the surrounding nodes of the next node are evaluated, and this operation is repeated until the end point;

[0056] As a preferred embodiment of this invention, in step S3, the obstacle detection method includes the following specific steps:

[0057] S3.2.1: Determine the nodes to be tested, such as Figure 4 As shown, the blue dot is the current node, and then the eight nodes to be tested around this node are placed in a sequence O;

[0058] S3.2.2: Select the nodes to be tested in sequence O one by one. To ensure that there is a sufficient safety distance between the car and the obstacle, use two grid units as the safety distance. If the line connecting the node to be tested and the node in the sequence touches the obstacle, delete the node from the sequence. Figure 4 If any node from 1 to 8 touches an obstacle, the node in the sequence will be deleted from sequence O;

[0059] S3.2.3: Repeat S3.2.2 for the selected nodes until there are no more nodes to be tested;

[0060] S4: Remove redundant nodes from the generated initial path to make the path smoother and finally obtain the optimized path;

[0061] As a preferred embodiment of this invention, in step S4, redundant nodes are removed from the generated initial path. The specific steps are as follows:

[0062] S4.1: If Figure 5As shown, the original route planning starting point start is determined first, and then the subsequent nodes ABC..I are connected in sequence;

[0063] S4.2: Determine whether the line connecting the two nodes touches an obstacle and whether it is outside the safe distance. In this example, the safe distance is set to 0.3m. If there is no contact, the current node is considered a redundant point. Otherwise, the previous node of the secondary node is added to the modified path node set. Figure 5 When Start is connected from A to F, it touches an obstacle, so point E before point F is added to the final set;

[0064] S4.3: Repeat the steps in S4.2 until you reach the end. Figure 5 Then we start from point E and go all the way to point End. The distance between the line connecting the two and the obstacle is less than the safe distance, so point I is added to the set. The final path produced is the green path in the figure.

[0065] like Figure 6 As shown, the first test of the A* method based on ground relief passes through three high ground relief nodes and one low ground relief node.

[0066] like Figure 7 As shown, the second test of the A* method based on ground relief passes through two high ground relief nodes and two low ground relief nodes.

[0067] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A path planning method for an orchard robot based on ground undulation adaptation, characterized in that: include: S1: Generate the corresponding ground relief map based on the pre-surveyed ground data; S2: Select the starting point and end point of the robot; S3: Use the A* algorithm based on ground relief and obstacle detection method to perform path planning and obtain the initial path; S4: Remove redundant nodes from the generated initial path to make the path smoother and finally obtain the optimized path.

2. The orchard robot path planning method based on ground undulation adaptation according to claim 1, characterized in that: The path planning method is provided with an evaluation function that can be adaptively adjusted according to the terrain undulation: F(n)=G(n)+k(n)·H(n) Where G(n) is the actual cost, which is the actual cost from the starting node to the pre-selected node; H(n)——estimated cost; k(n) — weight function; α n ——ground relief at the nth node; α k ——the average value of ground relief at all nodes; b——weight proportional coefficient.

3. The orchard robot path planning method based on ground undulation adaptation according to claim 2, characterized in that: The b-weight proportional coefficient is the ratio of the number of nodes with high ground relief to the number of nodes with low ground relief. Nodes with a ground relief of 0° are flat ground, nodes with a ground relief greater than 0° and less than 15° are considered low ground relief nodes, and nodes with a ground relief greater than or equal to 15° are considered high ground relief nodes.

4. The orchard robot path planning method based on ground undulation adaptation according to claim 1, characterized in that: The obstacle detection method includes: S3.1: Set a warning buffer zone with a width of two grids near the obstacle; S3.2: Place the eight test nodes around the current node into the sequence. If the line connecting these test nodes to the original node encounters an obstacle, delete this node from the sequence. S3.3: Repeat S3.2 for the filtered nodes until there are no more nodes to be tested.

5. The orchard robot path planning method based on ground undulation adaptation according to claim 1, characterized in that: The steps for removing redundant nodes from the generated initial path are as follows: determining the starting point of the original route planning, and then connecting the subsequent nodes in the path in sequence to determine whether the connection between the two touches an obstacle and whether it is outside the safe distance. If not, the current node is regarded as a redundant point. Otherwise, the previous node of the secondary node is added to the modified path node set, and this process is repeated until the end point is reached.

Images