A path planning method and control system for an orchard picking robot

Abstract

The present invention provides a path planning method and control system for an orchard picking robot, comprising the following steps: using the RRT* algorithm to derive an expansion tree T with the initial point as the starting point and the robot end point as the end point based on the robot's initial point, the robot's end point, and the expansion step size; determining a target offset probability λ; determining a random number; obtaining a sampling point; obtaining a new node; performing collision detection on the path between the node and the new node: establishing a spherical search space with a radius R with the new node as the center, with the nodes within the spherical search space set as neighboring nodes; calculating the Euclidean distance of all paths from the new node to the initial point via the neighboring nodes; reselecting a parent node: adding the new node to the expansion tree T; updating the expansion tree; and outputting a path P if the distance between the target point and the new node is less than the expansion step size. The present invention can quickly find a collision-free path and efficiently plan paths, facilitating real-time control of the picking robot.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic picking or path planning, and in particular to a path planning method and a control system for an orchard picking robot. Background Art

[0002] Fruit picking plays a very important role in fruit farmers' farming. Currently, the picking of fruits from trees such as citrus and apples mainly relies on manual labor. Fruit farmers need to use ladders to pick fruits above the trees. In addition, fruit farmers need to frequently move ladders to pick fruits from different directions of the same tree or from different trees. This production method is labor-intensive and inefficient.

[0003] With the advancement of mechanization, agricultural picking robots are gradually being used in fruit picking operations. Early fruit picking robots used methods such as shaking branches to pick fruits, which easily damaged the picked fruits.

[0004] Modern agricultural harvesting robots use various sensors to sense their targets and use the information they acquire to guide their robotic arms to perform precise picking operations. However, orchards are complex environments, and the obstruction of fruit from branches, leaves, and other objects can significantly reduce the efficiency of robotic arms. Summary of the Invention

[0005] In response to the deficiencies in the prior art, the present invention provides a path planning method and control system for an orchard picking robot, which can find the optimal path more conveniently and quickly, allowing the picking robot to quickly perform picking operations.

[0006] The present invention achieves the above technical objectives through the following technical means.

[0007] A path planning method for an orchard picking robot comprises the following steps:

[0008] S01: Determine the initial point x of the manipulator start , the end point of the manipulator and the expansion step Δl; according to the initial point x start The end point of the manipulator and the expansion step length Δl are obtained by using the RRT* algorithm with the initial point x start is the starting point and the end point of the manipulator is the expansion tree T; determine the target offset probability λ; let the end point of the manipulator be the target point x goal ;

[0009] S02: Use the rand function to randomly generate a random number between 0 and 1;

[0010] S03: Get sampling point X rand , specifically:

[0011] If the random number is less than λ, the target point x goal Set as sampling point X rand ;

[0012] If the random number is greater than λ, a random sampling point x is obtained by random sampling in the sampling space. r ' and The sampling space is a space consisting of the starting point, the end point, each node in the expansion tree T and various obstacles; the target point x goal For node x near Generates attraction, where node x near is the distance from the random sampling point x′ on the expanded tree T rand nearest point;

[0013] According to the target point x goal For node x near The attraction of the temporary path point x tp ;

[0014] According to the random sampling point x′ rand and temporary path point x tp Determine the sampling point X rand ;

[0015] S04: Find the new node x new ;

[0016] Take node x near As the starting point, Δl is the expansion step to generate a new node x new , new node x new There are 0 nodes waiting to be added to the extended tree T.

[0017]

[0018] S05: For node x near and the new node x new Collision detection is performed on the paths between them:

[0019] If the node x near and the new node x new If a collision occurs, repeat steps S02 to S04 until there is no collision;

[0020] S06: Determine node x near and the new node x new After there is no collision between them, the new node x new As the center, a spherical search space with a radius of R is established, and the nodes in the spherical search space are set as neighbor nodes;

[0021] S07: Calculate the new node x new Passing through neighboring nodes to the initial point x startThe Euclidean distance of all paths;

[0022] S08: Reselect the parent node:

[0023] Determine the new node x new Passing through neighboring nodes to the initial point x start The neighbor node x1 with the smallest European distance and no collision;

[0024] Set the neighbor node x1 as the new node x new The parent node is a node that can be extended downward on the expansion tree T;

[0025] S09: Add the new node x new Add to the extension tree T;

[0026] S10: If the parent node of the remaining neighboring nodes in the spherical search space is changed to the new node x new After that, the new node x new Passing through neighboring nodes to the initial point x start If the Euclidean distance of the neighbor node decreases, the parent node of the neighbor node is changed to the new node x. new ;

[0027] S11: Update the extension tree.

[0028] S12: If the target point x goal and the new node x new The distance between them is greater than the expansion step Δl, then return to S02 to continue; if the target point x goal and the new node x new If the distance between them is less than the expansion step Δl, the path P is output.

[0029] Furthermore, according to the random sampling point x′ rand and temporary path point x tp Determine the sampling point X rand , specifically:

[0030]

[0031] Further, according to the target point x goal For node x near The attraction magnitude is used to get the temporary path point x tp , as follows:

[0032]

[0033] where k att is the gravitational gain.

[0034] Furthermore, the radius R of the search space is calculated according to the following formula:

[0035]

[0036] Where:

[0037] R is the radius of the search space;

[0038] γ is a planning constant based on the environment;

[0039] n represents the number of neighboring nodes; d represents the dimension of the planning space.

[0040] A control system for path planning of an orchard picking robot comprises a desired trajectory module, a control module and a robot; the desired trajectory module integrates a program for the path planning method of the orchard picking robot; the desired trajectory module inputs a desired path P into the control module; the control module converts the desired path P into an angle for rotation and a displacement for movement of the robot, and determines the response time of a drive unit on the robot according to a set movement speed; the control module controls the robot to move according to the desired path P.

[0041] The beneficial effects of the present invention are:

[0042] The path planning method and control system for an orchard picking robot described in the present invention obtain a fast search random tree algorithm based on the concept of gravity and the target deflection probability optimization RRT* algorithm. The algorithm can find a collision-free path in a short time and has high path planning efficiency, which is conducive to the real-time control of the picking robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. The drawings described below are some embodiments of the present invention. For ordinary technicians in this field, it is obvious that other drawings can be obtained based on these drawings without paying any creative work.

[0044] Figure 1 This is a flow chart of the path planning method for the orchard picking robot described in the present invention.

[0045] Figure 2 This is the random tree expansion graph of the present invention where the random number is less than λ.

[0046] Figure 3 The invention provides a random tree expansion graph in which the random number is greater than λ. DETAILED DESCRIPTION

[0047] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but the protection scope of the present invention is not limited thereto.

[0048] The following describes embodiments of the present invention in detail, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and are not to be construed as limiting the present invention.

[0049] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "axial", "radial", "vertical", "horizontal", "inside", "outside" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first" and "second" are used for descriptive purposes only, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, "multiple" means two or more, unless otherwise clearly and specifically defined.

[0050] In the present invention, unless otherwise expressly specified or limited, the terms "mounted," "connected," "connect," "fixed," etc. should be understood broadly. For example, they may refer to fixed, detachable, or integral connections; mechanical or electrical connections; direct or indirect connections through an intermediary; or internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0051] like Figure 1 As shown, the path planning method of the orchard picking robot described in the present invention includes the following steps:

[0052] S01: Determine the initial point x of the manipulator start , the end point of the manipulator and the expansion step Δl; according to the initial point x start The end point of the manipulator and the expansion step length Δl are obtained by using the RRT* algorithm with the initial point x start is the starting point and the end point of the manipulator is the expansion tree T; determine the target offset probability λ; let the end point of the manipulator be the target point x goal ;like Figure 2 As shown, in the embodiment, there are nodes of the expansion tree T with serial numbers 2, 3...8 on the expansion tree T.

[0053] S02: Use the rand function to randomly generate a random number between 0 and 1;

[0054] S03: Get sampling point X rand , specifically:

[0055] If the random number is less than λ, the target point x goal Set as sampling point X rand ,like Figure 2 shown.

[0056] If the random number is greater than λ, a random sampling point x′ is obtained by random sampling in the sampling space. rand The sampling space is a space consisting of the starting point, the end point, each node in the expansion tree T and various obstacles; the target point x goal For node x near Generates attraction, where node x near is the distance from the random sampling point x′ on the expanded tree T rand nearest point;

[0057] According to the target point x goal For node x near The attraction of the temporary path point x tp , as follows:

[0058]

[0059] where k att is the gravitational gain;

[0060] According to the random sampling point x′ rand and temporary path point x tp Determine the sampling point X rand , specifically:

[0061]

[0062] S04: Find the new node x new ;

[0063] Take node x near As the starting point, Δl is the expansion step to generate a new node x new , new node x new A node waiting to be added to the extended tree T,

[0064]

[0065] S05: For node x near and the new node x new Collision detection is performed on the paths between them:

[0066] If the node x near and the new node x newIf a collision occurs, repeat steps S02 to S04 until there is no collision; that is, node x near For the new node x new neighboring nodes.

[0067] S06: Determine node x near and the new node x new After there is no collision between them, the new node x new A spherical search space with a radius of R is established with centered on , and the nodes in the spherical search space are set as neighboring nodes. The radius R of the search space is calculated according to the following formula:

[0068]

[0069] Where:

[0070] R is the radius of the search space;

[0071] γ is a planning constant based on the environment;

[0072] n represents the number of neighboring nodes; d represents the dimension of the planning space.

[0073] by Figure 3 For example, take the new node x new The existence of nodes number 7 and 8 in the spherical search space with radius R,

[0074] S07: Calculate the new node x new Passing through neighboring nodes to the initial point x start The Euclidean distance of all paths; Figure 3 For example, the initial point x start To the new node x new There are 2 paths, the first one is x start -4-5-6-7-x near -x new , the other is x start -4–5–6–8-x near -x new , and then calculate the Euclidean distance of each path.

[0075] S08: Reselect the parent node:

[0076] Determine the new node x new Passing through neighboring nodes to the initial point x start The nearest neighbor node x1 with the smallest European distance and no collision; the nearest neighbor node x1 is the distance from the new node x in the path new The nearest node. Figure 3 x in near is the distance from the new node x new The nearest node.

[0077] Set the neighbor node x1 as the new node x new The parent node is a node that can be extended downward on the expansion tree T; Figure 3 In node 7, the parent node of node 8 is node 6.

[0078] S09: Add the new node x new Add to the extension tree T;

[0079] S10: If the parent node of the remaining neighboring nodes in the spherical search space is changed to the new node x new After that, the new node x new Passing through neighboring nodes to the initial point x start If the Euclidean distance of the neighbor node decreases, the parent node of the neighbor node is changed to the new node x. new ;

[0080] S11: Update the extension tree.

[0081] S12: If the target point x goal and the new node x new The distance between them is greater than the expansion step Δl, then return to S02 to continue; if the target point x goal and the new node x new If the distance between them is less than the expansion step Δl, the path P is output.

[0082] A control system for path planning of an orchard picking robot comprises a desired trajectory module, a control module and a robot; the desired trajectory module integrates a program of the path planning method for an orchard picking robot described in the present invention, the desired trajectory module inputs a desired path P into the control module, the control module converts the desired path P into an angle for rotation and a displacement for movement of the robot, and determines the response time of a drive unit on the robot according to a set movement speed; the control module controls the robot to move according to the desired path P.

[0083] It should be understood that although this specification is described according to various embodiments, not every embodiment contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

[0084] The series of detailed descriptions listed above are only specific descriptions of feasible embodiments of the present invention. They are not intended to limit the scope of protection of the present invention. Any equivalent embodiments or changes that do not deviate from the technical spirit 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 picking robot, characterized in that: The steps include: S01: Determine the initial point of the robot , robot end point and extension step ; According to the initial point of the robot , robot end point and extension step Using the RRT* algorithm, we can get the initial point As the starting point and the end point of the manipulator as the expansion tree T; determine the target offset probability λ; let the end point of the manipulator be the target point ; S02: Use the rand function to randomly generate a random number between 0 and 1; S03: Get sampling points , specifically: If the random number is less than λ, the target point Set as sampling point ; If the random number is greater than λ, a random sampling point is obtained by random sampling in the sampling space. The sampling space is a space consisting of the starting point, the end point, each node in the expansion tree T and various obstacles; the target point For Node Generates attraction, where the nodes is the distance between random sampling points on the extended tree T nearest point; According to the target point For Node The attraction size is obtained to obtain the temporary path point , specifically: ， in is the gravitational gain; According to random sampling points and temporary waypoints Determine sampling points , specifically: ； S04: Find new nodes ; Node As a starting point, Generate new nodes for expansion steps , new node A node waiting to be added to the extended tree T, ； S05: Node and new nodes Collision detection is performed on the paths between them: If the node and new nodes If a collision occurs, repeat steps S02 to S04 until there is no collision; S06: Determine the node and new nodes After there is no collision between the new nodes As the center, a spherical search space with a radius of R is established, and the nodes in the spherical search space are set as neighbor nodes; S07: Calculate new nodes Passing through neighboring nodes to the initial point The Euclidean distance of all paths; S08: Reselect the parent node: Determine at the new node Passing through neighboring nodes to the initial point Neighbor nodes with the smallest European distance and no collision ; Nearest neighbor nodes Set as new node The parent node is a node that can be extended downward on the expansion tree T; S09: New node Add to the extension tree T; S10: If the parent node of the remaining neighboring nodes in the spherical search space is changed to a new node After that, the new node can be Passing through neighboring nodes to the initial point If the Euclidean distance of the neighbor node decreases, the parent node of the neighbor node is changed to the new node. ; S11: Update the extension tree; S12: If the target point and new nodes The distance between them is greater than the expansion step , then return to S02 and continue; if the target point and new nodes The distance between them is less than the expansion step , then output path P.

2. The path planning method for an orchard picking robot according to claim 1, characterized in that: The radius R of the search space is calculated according to the following formula: ； Where: R is the radius of the search space; It is a planning constant based on the environment; Indicates the number of neighboring nodes; Represents the dimension of the planning space.

3. A control system for path planning of an orchard picking robot, characterized in that: The method comprises a desired trajectory module, a control module, and a manipulator; the desired trajectory module integrates a program of the path planning method for an orchard picking manipulator according to any one of claims 1 to 2; the desired trajectory module inputs a desired path P into the control module; the control module converts the desired path P into an angle of rotation and a displacement required for the manipulator to move, and determines the response time of a drive unit on the manipulator according to a set movement speed; The control module controls the robot to move according to the desired path P.

Images