Path planning method and apparatus for tomato inspection robot, device, medium, and product

1 . A path planning method for a tomato inspection robot, comprising:

obtaining a two-dimensional (2D) raster map of a tomato orchard, and determining start and end points in the 2D raster map and a search radius of the tomato inspection robot;

under a current iteration ordinal number, calculating a cost of a positive node under the current iteration ordinal number based on a heuristic function under a previous iteration ordinal number, and updating a weight of the heuristic function under the previous iteration ordinal number based on the cost of the positive node under the current iteration ordinal number to obtain a heuristic function under the current iteration ordinal number, wherein the cost is a sum of a corresponding heuristic function value from the positive node under the current iteration ordinal number to a negative node under the current iteration ordinal number, a corresponding heuristic function value from the positive node under the current iteration ordinal number to the end point, and a corresponding heuristic function value from the positive node under the current iteration ordinal number to the start point; the heuristic function is a weighted sum of a Euclidean distance and a diagonal distance between two nodes; and a positive node under an initial iteration ordinal number is the start point, and a negative node under the initial iteration ordinal number is the end point;

determining a positive safety distance based on whether there is an obstacle within a circle with the positive node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius, and determining a negative safety distance based on whether there is an obstacle within a circle with the negative node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius;

determining an eight neighborhood for the positive node and the negative node under the current iteration ordinal number based on relative positions of the positive node and the negative node under the current iteration ordinal number;

calculating a corresponding heuristic function value from each node in a positive target node set to the positive node under the current iteration ordinal number, as well as a corresponding heuristic function value from each node in a negative target node set to the negative node under the current iteration ordinal number based on the heuristic function under the current iteration ordinal number to obtain a positive heuristic function value set and a negative heuristic function value set, wherein the positive target node set comprises all nodes whose distances from the positive node under the current iteration ordinal number are greater than the positive safety distance in the eight neighborhood of the positive node under the current iteration ordinal number, and the negative target node set comprises all nodes whose distances from the negative node under the current iteration ordinal number are greater than the negative safety distance in the eight neighborhood of the negative node under the current iteration ordinal number;

determining a positive node and a negative node under a next iteration ordinal number based on the positive heuristic function value set and the negative heuristic function value set;

if there is no obstacle on a direct connection path between the positive node and the negative node under the next iteration ordinal number, performing backward tracing from the positive node under the next iteration ordinal number to the start point and from the negative node under the next iteration ordinal number to the end point to obtain a planned path, wherein the direct connection path is a path formed through direct connection; and

if there is an obstacle on the direct connection path between the positive node and the negative node under the next iteration ordinal number, updating an iteration ordinal number, and entering a next iteration.

2 . The path planning method for a tomato inspection robot according to claim 1 , wherein the heuristic function is as follows:

d=w1×d1+w2×d2+w3×2.828×min(dx,dy)+w4×2.236×min(dx,dy), wherein d represents the heuristic function value, d1 represents a diagonal distance between a 1 st node and a 2 nd node, d2 represents a Euclidean distance between the 1 st node and the 2 nd node, dx represents an absolute value of a difference between an abscissa of the 1 st node and an abscissa of the 2 nd node, dy represents an absolute value of a difference between an ordinate of the 1 st node and an ordinate of the 2 nd node, min(dx, dy) represents a minimum value between the dx and the dy, w1 represents a first weight, w2 represents a second weight, w3 represents a third weight, and w4 represents a fourth weight.

3 . The path planning method for a tomato inspection robot according to claim 1 , wherein the determining a positive safety distance based on whether there is an obstacle within a circle with the positive node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius comprises:

if there is an obstacle within the circle with the positive node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, increasing the positive safety distance; or

if there is no obstacle within the circle with the positive node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, decreasing the positive safety distance.

4 . The path planning method for a tomato inspection robot according to claim 1 , wherein the determining a negative safety distance based on whether there is an obstacle within a circle with the negative node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius specifically comprises:

if there is an obstacle within the circle with the negative node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, increasing the negative safety distance; or

if there is no obstacle within the circle with the negative node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, decreasing the negative safety distance.

5 . The path planning method for a tomato inspection robot according to claim 1 , wherein the determining an eight neighborhood for the positive node and the negative node under the current iteration ordinal number based on relative positions of the positive node and the negative node under the current iteration ordinal number specifically comprises:

establishing a positive coordinate system with the positive node under the current iteration ordinal number as an origin;

obtaining the eight neighborhood for the positive node under the current iteration ordinal number based on an eight neighborhood template corresponding to a quadrant in which the negative node under the current iteration ordinal number is located in the positive coordinate system;

establishing a negative coordinate system with the negative node under the current iteration ordinal number as an origin; and

obtaining the eight neighborhood for the negative node under the current iteration ordinal number based on an eight neighborhood template corresponding to a quadrant in which the positive node under the current iteration ordinal number is located in the negative coordinate system.

6 . The path planning method for a tomato inspection robot according to claim 1 , wherein the performing backward tracing from the positive node under the next iteration ordinal number to the start point and from the negative node under the next iteration ordinal number to the end point to obtain a planned path specifically comprises:

performing the backward tracing from the positive node under the next iteration ordinal number to the start point and from the negative node under the next iteration ordinal number to the end point to obtain a preliminary path; and

smoothing the preliminary path to obtain the planned path.

7 . A path planning apparatus for a tomato inspection robot, comprising:

an obtaining module configured to obtain a 2D raster map of a tomato orchard, and determine start and end points in the 2D raster map and a search radius of the tomato inspection robot;

an updating module configured to: under a current iteration ordinal number, calculate a cost of a positive node under the current iteration ordinal number based on a heuristic function under a previous iteration ordinal number, and update a weight of the heuristic function under the previous iteration ordinal number based on the cost of the positive node under the current iteration ordinal number to obtain a heuristic function under the current iteration ordinal number, wherein the cost is a sum of a corresponding heuristic function value from the positive node under the current iteration ordinal number to a negative node under the current iteration ordinal number, a corresponding heuristic function value from the positive node under the current iteration ordinal number to the end point, and a corresponding heuristic function value from the positive node under the current iteration ordinal number to the start point; the heuristic function is a weighted sum of a Euclidean distance and a diagonal distance between two nodes; and a positive node under an initial iteration ordinal number is the start point, and a negative node under the initial iteration ordinal number is the end point;

a safety distance determining module configured to determine a positive safety distance based on whether there is an obstacle within a circle with the positive node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius, and determine a negative safety distance based on whether there is an obstacle within a circle with the negative node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius;

an eight neighborhood determining module configured to determine an eight neighborhood for the positive node and the negative node under the current iteration ordinal number based on relative positions of the positive node and the negative node under the current iteration ordinal number;

a heuristic function value set calculation module configured to: calculate a corresponding heuristic function value from each node in a positive target node set to the positive node under the current iteration ordinal number, as well as a corresponding heuristic function value from each node in a negative target node set to the negative node under the current iteration ordinal number based on the heuristic function under the current iteration ordinal number to obtain a positive heuristic function value set and a negative heuristic function value set, wherein the positive target node set comprises all nodes whose distances from the positive node under the current iteration ordinal number are greater than the positive safety distance in the eight neighborhood of the positive node under the current iteration ordinal number, and the negative target node set comprises all nodes whose distances from the negative node under the current iteration ordinal number are greater than the negative safety distance in the eight neighborhood of the negative node under the current iteration ordinal number;

a module for determining a node under a next iteration ordinal number configured to determine a positive node and a negative node under the next iteration ordinal number based on the positive heuristic function value set and the negative heuristic function value set;

a planned path determining module configured to: if there is no obstacle on a direct connection path between the positive node and the negative node under the next iteration ordinal number, perform backward tracing from the positive node under the next iteration ordinal number to the start point and from the negative node under the next iteration ordinal number to the end point to obtain a planned path, wherein the direct connection path is a path formed through direct connection; and

an iteration module configured to: if there is an obstacle on the direct connection path between the positive node and the negative node under the next iteration ordinal number, update an iteration ordinal number, and enter a next iteration.

8 . A computer device, comprising: a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the path planning method for a tomato inspection robot according to claim 1 .

9 . A non-transitory computer-readable storage medium, wherein a computer program is stored thereon, and the computer program is executed by a processor to implement the path planning method for a tomato inspection robot according to claim 1 .

10 . The computer device according to claim 8 , wherein the heuristic function is as follows:

d=w1×d1+w2×d2+w3×2.828×min(dx,dy)+w4×2.236×min(dx,dy), wherein d represents the heuristic function value, d1 represents a diagonal distance between a 1 st node and a 2 nd node, d2 represents a Euclidean distance between the 1 st node and the 2 nd node, dx represents an absolute value of a difference between an abscissa of the 1 st node and an abscissa of the 2 nd node, dy represents an absolute value of a difference between an ordinate of the 1 st node and an ordinate of the 2 nd node, min(dx, dy) represents a minimum value between the dx and the dy, w1 represents a first weight, w2 represents a second weight, w3 represents a third weight, and w4 represents a fourth weight.

11 . The computer device according to claim 8 , wherein the determining a positive safety distance based on whether there is an obstacle within a circle with the positive node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius comprises:

if there is an obstacle within the circle with the positive node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, increasing the positive safety distance; or

if there is no obstacle within the circle with the positive node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, decreasing the positive safety distance.

12 . The computer device according to claim 8 , wherein the determining a negative safety distance based on whether there is an obstacle within a circle with the negative node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius specifically comprises:

if there is an obstacle within the circle with the negative node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, increasing the negative safety distance; or

if there is no obstacle within the circle with the negative node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, decreasing the negative safety distance.

13 . The computer device according to claim 8 , wherein the determining an eight neighborhood for the positive node and the negative node under the current iteration ordinal number based on relative positions of the positive node and the negative node under the current iteration ordinal number specifically comprises:

establishing a positive coordinate system with the positive node under the current iteration ordinal number as an origin;

obtaining the eight neighborhood for the positive node under the current iteration ordinal number based on an eight neighborhood template corresponding to a quadrant in which the negative node under the current iteration ordinal number is located in the positive coordinate system;

establishing a negative coordinate system with the negative node under the current iteration ordinal number as an origin; and

obtaining the eight neighborhood for the negative node under the current iteration ordinal number based on an eight neighborhood template corresponding to a quadrant in which the positive node under the current iteration ordinal number is located in the negative coordinate system.

14 . The computer device according to claim 8 , wherein the performing backward tracing from the positive node under the next iteration ordinal number to the start point and from the negative node under the next iteration ordinal number to the end point to obtain a planned path specifically comprises:

performing the backward tracing from the positive node under the next iteration ordinal number to the start point and from the negative node under the next iteration ordinal number to the end point to obtain a preliminary path; and

smoothing the preliminary path to obtain the planned path.

15 . The non-transitory computer-readable storage medium according to claim 9 , wherein the heuristic function is as follows:

d=w1×d1+w2×d2+w3×2.828×min(dx,dy)+w4×2.236×min(dx,dy), wherein d represents the heuristic function value, d1 represents a diagonal distance between a 1 st node and a 2 nd node, d2 represents a Euclidean distance between the 1 st node and the 2 nd node, dx represents an absolute value of a difference between an abscissa of the 1 st node and an abscissa of the 2 nd node, dy represents an absolute value of a difference between an ordinate of the 1 st node and an ordinate of the 2 nd node, min(dx, dy) represents a minimum value between the dx and the dy, w1 represents a first weight, w2 represents a second weight, w3 represents a third weight, and w4 represents a fourth weight.

16 . The non-transitory computer-readable storage medium according to claim 9 , wherein the determining a positive safety distance based on whether there is an obstacle within a circle with the positive node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius comprises:

if there is an obstacle within the circle with the positive node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, increasing the positive safety distance; or

if there is no obstacle within the circle with the positive node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, decreasing the positive safety distance.

17 . The non-transitory computer-readable storage medium according to claim 9 , wherein the determining a negative safety distance based on whether there is an obstacle within a circle with the negative node under the current iteration ordinal number as a center and the search radius of the tomato inspection robot as a radius specifically comprises:

if there is an obstacle within the circle with the negative node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, increasing the negative safety distance; or

if there is no obstacle within the circle with the negative node under the current iteration ordinal number as the center and the search radius of the tomato inspection robot as the radius, decreasing the negative safety distance.

18 . The non-transitory computer-readable storage medium according to claim 9 , wherein the determining an eight neighborhood for the positive node and the negative node under the current iteration ordinal number based on relative positions of the positive node and the negative node under the current iteration ordinal number specifically comprises:

establishing a positive coordinate system with the positive node under the current iteration ordinal number as an origin;

obtaining the eight neighborhood for the positive node under the current iteration ordinal number based on an eight neighborhood template corresponding to a quadrant in which the negative node under the current iteration ordinal number is located in the positive coordinate system;

establishing a negative coordinate system with the negative node under the current iteration ordinal number as an origin; and

obtaining the eight neighborhood for the negative node under the current iteration ordinal number based on an eight neighborhood template corresponding to a quadrant in which the positive node under the current iteration ordinal number is located in the negative coordinate system.

19 . The non-transitory computer-readable storage medium according to claim 9 , wherein the performing backward tracing from the positive node under the next iteration ordinal number to the start point and from the negative node under the next iteration ordinal number to the end point to obtain a planned path specifically comprises:

performing the backward tracing from the positive node under the next iteration ordinal number to the start point and from the negative node under the next iteration ordinal number to the end point to obtain a preliminary path; and

smoothing the preliminary path to obtain the planned path.