Mechanical arm obstacle avoidance path planning method and device, equipment and medium

Abstract

The application relates to a mechanical arm obstacle avoidance path planning method, device, equipment and medium, the method comprises the following steps: in view of the low efficiency problem of RRT global random exploration, an adaptive spherical sampling space is used to limit the exploration area and improve the search efficiency, and the problem that the planning fails due to falling into a local maximum value is avoided; in view of the slow convergence speed of the sampling algorithm, the target gravity is introduced, the search is more directional, the convergence speed is improved, the cost function in the A* algorithm is fused, and the search efficiency and the convergence speed are further improved. The application can realize efficient obstacle avoidance, high-quality path generation and real-time response to a dynamic environment, so that a more reliable and efficient path planning scheme is provided for a mechanical arm picking task.

Description

Technical Field

[0001] This application relates to the field of obstacle avoidance path planning, and in particular to a method for planning obstacle avoidance path for a robotic arm, a corresponding device, electronic equipment, and a computer-readable storage medium. Background Technology

[0002] With the continuous development of automation technology, robotic arms are increasingly widely used in agriculture, especially in harvesting operations in farmland, where they have shown great potential. The high precision and flexibility of robotic arms make them an ideal harvesting tool, effectively improving agricultural production efficiency. However, the path planning problem faced by robotic arms during harvesting has become a significant challenge limiting their application.

[0003] In the process of robotic arm harvesting, traditional path planning methods such as artificial potential field method and RRT method have a series of drawbacks, which limit the performance of robotic arms in practical applications.

[0004] First, the artificial potential field method is prone to getting stuck in local minima when robotic arms are harvesting and avoiding obstacles. Due to environmental complexity and the need for multiple robotic arms to work together, the artificial potential field method leads to robotic arms choosing less flexible or efficient paths, affecting harvesting efficiency. Furthermore, this method is poorly adaptable to dynamic environments and cannot handle obstacles that change in real time, thus reducing the applicability of robotic arms in real farmland environments.

[0005] Secondly, while the RRT method has made significant progress in path planning, it still faces some challenges in robotic arm harvesting, as the generated paths may be suboptimal, especially in high-dimensional state spaces. Furthermore, the RRT method encounters problems such as high computational complexity, long planning times, and insufficient path quality when dealing with dynamic environments and scenarios requiring high accuracy, thus challenging its feasibility in agricultural harvesting tasks.

[0006] In summary, existing technologies, such as the artificial potential field method, result in robotic arms that are not flexible or efficient enough in selecting paths, affecting harvesting efficiency. In addition, the RRT method faces problems such as high computational complexity, long planning time, and insufficient path quality when dealing with dynamic environments and scenarios with high accuracy requirements. The applicant has made corresponding explorations to address these issues. Summary of the Invention

[0007] The purpose of this application is to solve the above-mentioned problems by providing a robotic arm obstacle avoidance path planning method, corresponding device, electronic device and computer-readable storage medium.

[0008] To achieve the various objectives of this application, the following technical solution is adopted:

[0009] A robotic arm obstacle avoidance path planning method proposed to meet one of the purposes of this application includes:

[0010] In response to the obstacle avoidance path planning command of the robotic arm, obtain the starting point q of the robotic arm's execution path. start and the current expansion node q expand Construct a system based on the current expansion node q expand A spherical space with center r and radius r is used as the sampling space for random sampling of a preset threshold number of points to determine the random sampling point sequence q. sequence ;

[0011] For the random sampling point sequence q sequence Each random sampling point q in rand The random sampling points are sorted in ascending order of cost using a preset cost function to determine the sorted sequence.

[0012] Calculate and determine all nodes in the extended tree and random sampling points q in the sorted sequence of random sampling points. rand The Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near ;

[0013] Based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new ;

[0014] Determine the new node q new The node q closest to the random sampling point near The path between them is checked to see if it collides with any obstacles. If no collision occurs, the new node q is moved to the next node. new Add it to the extended tree as a child node of the node closest to the random sampling point, and set the new node q new As the extension node q in the next cycle expand ;

[0015] Calculate and determine the new node q new The distance between each node in the extended tree, when there exists a node q in the extended tree such that ||q|| new If -q||+cost(q) is minimized, then node q is set to q. new The parent node, where ||q new -q|| represents the Euclidean distance between the two, and cost(q) is the path length from q to the starting point;

[0016] Determine the new node q new With the extended node q expand Check whether the Euclidean distance between them is less than the local maximum / minimum threshold m, and calculate the number of times it is less than the local maximum / minimum threshold m. When the number is greater than t, adjust the exploration space radius and set the exploration space radius to twice the previous exploration space radius r. After jumping out of the local maximum / minimum region, set the exploration space radius to half of the previous exploration space radius r in turn until it is restored to the original sampling space radius.

[0017] Detect the new node q new If the Euclidean distance between the target point and the target point is less than or equal to the deviation threshold ε, then the newly expanded node obtained in the last iteration is taken as the parent node of the endpoint, and the newly expanded node obtained in the last iteration is taken as the child node. The process continues to backtrack through the parent node in the expansion tree until it returns to the starting point q. start ;

[0018] The parent nodes found through backtracking are sorted in a preset order to determine the final path nodes. Based on the final path nodes, the obstacle avoidance planning path is determined to complete the obstacle avoidance path planning of the robotic arm.

[0019] Optionally, for the random sampling point sequence q sequence Each random sampling point q in rand The steps for determining the sorted random sampling point sequence by sorting the samples according to their costs from smallest to largest using a preset cost function include:

[0020] The preset cost function is:

[0021] f(q) = g(q) + k·h(q),

[0022] g(q) = cost(q) rand ),

[0023] h(q) = ||q rand -q goal ||,

[0024] Where cost(q) rand ) represents a random sampling point q rand To the starting point q start The path length, ||q rand -q goal ||for q rand and the endpoint q goal The Euclidean distance, where k is the coefficient of the cost function.

[0025] Optionally, based on the random sampling point q randAnd the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new The steps include:

[0026] The formula for calculating the gravitational force at a random sampling point is:

[0027]

[0028] Among them, U att K represents the gravitational force at random sampling points. att ρ(q) is the scale factor of gravity. rand ,q goal ) represents a random sampling point q rand To the destination q goal Euclidean distance;

[0029] Based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand The calculation formula is as follows:

[0030] q rand =q new_rand +U att ,

[0031] Wherein, the q rand Characterizing random sampling points, the q new_rand The updated random sampling points are represented.

[0032] Optionally, based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new The steps include:

[0033] Based on the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new The calculation formula is as follows:

[0034]

[0035] Optionally, determine the new node q newThe node q closest to the random sampling point near The step of detecting whether the path collides with an obstacle includes:

[0036] The new node q was detected. new The node q closest to the random sampling point near If the path between them collides with an obstacle, the calculation is re-performed to determine all nodes in the extended tree and the random sampling point q in the sorted random sampling point sequence. rand The Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near This step.

[0037] Optionally, after sorting the parent nodes found through backtracking in a preset order to determine the final path nodes, the process includes:

[0038] Determine the preset step length of the robotic arm corresponding to the harvesting robot and the initial position of the end effector of the robotic arm;

[0039] Based on the final path node and the initial position of the robotic arm's end effector, the motion direction of the robotic arm's end effector is calculated and determined;

[0040] The picking position of the robotic arm's end effector is calculated based on the initial position of the end effector, the preset step length, and the direction of movement of the end effector, so as to complete the obstacle avoidance path planning of the robotic arm.

[0041] Optionally, the parent nodes found through backtracking are sorted according to a preset order to determine the final path nodes. Based on the final path nodes, an obstacle avoidance planning path is determined to complete the obstacle avoidance path planning steps for the robotic arm, including:

[0042] In response to the fruit picking command, the least squares method is used to fit the final path nodes to generate a smooth obstacle avoidance planning path for the robotic arm.

[0043] The harvesting robot harvests fruit based on the obstacle avoidance path planned by the robotic arm, thus completing the obstacle avoidance path planning of the robotic arm.

[0044] A robotic arm obstacle avoidance path planning device provided for another purpose of this application includes:

[0045] The sampling point determination module is configured to respond to the robotic arm's obstacle avoidance path planning command and obtain the starting point q of the robotic arm's execution path. start and the current expansion node q expand Construct a system based on the current expansion node q expandA spherical space with center r and radius r is used as the sampling space for random sampling of a preset threshold number of points to determine the random sampling point sequence q. sequence ;

[0046] The sampling point sorting module is configured to sort the random sampling point sequence q. sequence Each random sampling point q in rand The random sampling points are sorted in ascending order of cost using a preset cost function to determine the sorted sequence.

[0047] The nearest node determination module is configured to calculate and determine the relationship between all nodes in the extended tree and random sampling points q in the sorted sequence of random sampling points. rand The Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near ;

[0048] The new node determination module is configured to be based on the random sampling point q. rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new ;

[0049] The collision test module is configured to determine the new node q. new The node q closest to the random sampling point near The path between them is checked to see if it collides with any obstacles. If no collision occurs, the new node q is moved to the next node. new Add it to the extended tree as a child node of the node closest to the random sampling point, and set the new node q new As the extension node q in the next cycle expand ;

[0050] The parent node determination module is configured to calculate and determine the new node q. new The distance between each node in the extended tree, when there exists a node q in the extended tree such that ||q|| new If -q||+cost(q) is minimized, then node q is set to q. new The parent node, where ||q new -q|| represents the Euclidean distance between the two, and cost(q) is the path length from q to the starting point;

[0051] Explore the radius adjustment module, set it to determine the new node q new With the extended node qexpand Check whether the Euclidean distance between them is less than the local maximum / minimum threshold m, and calculate the number of times it is less than the local maximum / minimum threshold m. When the number is greater than t, adjust the exploration space radius and set the exploration space radius to twice the previous exploration space radius r. After jumping out of the local maximum / minimum region, set the exploration space radius to half of the previous exploration space radius r in turn until it is restored to the original sampling space radius.

[0052] The path backtracking module is configured to detect the new node q. new If the Euclidean distance between the target point and the target point is less than or equal to the deviation threshold ε, then the newly expanded node obtained in the last iteration is taken as the parent node of the endpoint, and the newly expanded node obtained in the last iteration is taken as the child node. The process continues to backtrack through the parent node in the expansion tree until it returns to the starting point q. start ;

[0053] The obstacle avoidance path determination module is configured to sort the parent nodes found through backtracking in a preset order, determine the final path node, and determine the obstacle avoidance planning path based on the final path node to complete the obstacle avoidance path planning of the robotic arm.

[0054] An electronic device provided for another purpose of this application includes a central processing unit and a memory, the central processing unit being configured to invoke and run a computer program stored in the memory to perform the steps of the robotic arm obstacle avoidance path planning method of this application.

[0055] A computer-readable storage medium is provided for another purpose of this application, which stores, in the form of computer-readable instructions, a computer program implemented according to the robotic arm obstacle avoidance path planning method, which, when called by a computer, executes the steps included in the corresponding method.

[0056] Compared to existing technologies, this application addresses the problems of insufficient flexibility or efficiency in path selection for robotic arms due to the artificial potential field method, which affects harvesting efficiency, and the high computational complexity, long planning time, and insufficient path quality faced by the RRT method when dealing with dynamic environments and scenarios with high accuracy requirements. This application provides, but is not limited to, the following beneficial effects:

[0057] Firstly, to address the low efficiency of global random exploration in RRT, this application proposes an adaptive spherical sampling space to limit the exploration area and improve search efficiency, while avoiding the problem of getting trapped in local maxima and causing planning failure. To address the slow convergence speed of the sampling algorithm, target gravity is introduced to make the search more directional and improve the convergence speed. This application integrates the cost function in the A* algorithm to further improve search efficiency and convergence speed.

[0058] Secondly, the obstacle avoidance path planning method of the present application reduces the average path length by 16.2%, the planning time by 79.3%, the average number of path nodes by 45.2%, and the number of iterations by 90.5%. The obstacle avoidance path planning method of the present application can effectively improve search efficiency and convergence speed, and can quickly generate high-quality paths.

[0059] Thirdly, the obstacle avoidance path planning method of the robotic arm in this application can greatly improve the applicability and efficiency of the robotic arm in agricultural harvesting operations. By adopting advanced path planning strategies, this application can achieve efficient obstacle avoidance, high-quality path generation, and real-time response to dynamic environments, thereby providing a more reliable and efficient path planning solution for robotic arm harvesting tasks. Attached Figure Description

[0060] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein:

[0061] Figure 1 This is a flowchart illustrating the obstacle avoidance path planning method for the robotic arm in the embodiments of this application;

[0062] Figure 2 This is a schematic diagram of the structure of the computer device in the embodiments of this application. Detailed Implementation

[0063] The embodiments of this application are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain this application, and should not be construed as limiting this application.

[0064] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the term “comprising” as used in this application means the presence of the stated features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. It should be understood that when we say an element is “connected” or “coupled” to another element, it can be directly connected or coupled to the other element, or there may be intermediate elements. Furthermore, “connected” or “coupled” as used herein can include wireless connections or wireless coupling. The term “and / or” as used herein includes all or any units and all combinations of one or more associated listed items.

[0065] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless specifically defined as herein.

[0066] Those skilled in the art will understand that the terms "client," "terminal," and "terminal device" as used herein include both devices that receive wireless signals, devices that only possess wireless signal receiver capabilities without transmission capabilities, and devices with receiving and transmitting hardware, devices that have receiving and transmitting hardware capable of bidirectional communication over a bidirectional communication link. Such devices may include: cellular or other communication devices such as personal computers or tablets, having single-line displays, multi-line displays, or cellular or other communication devices without multi-line displays; PCS (Personal Communications Service) that can combine voice, data processing, fax, and / or data communication capabilities; PDA (Personal Digital Assistant) that may include a radio frequency receiver, pager, internet / intranet access, web browser, notepad, calendar, and / or GPS (Global Positioning System) receiver; and conventional laptops and / or handheld computers or other devices that have and / or include radio frequency receivers. As used herein, "client," "terminal," and "terminal device" can be portable, transportable, installed in a means of transportation (air, sea, and / or land), or suitable and / or configured to operate locally and / or in a distributed manner, operating in any other location on Earth and / or in space. "Client," "terminal," and "terminal device" as used herein can also be a communication terminal, an internet access terminal, or a music / video playback terminal, such as a PDA, a MID (Mobile Internet Device), and / or a mobile phone with music / video playback capabilities, or a smart TV, set-top box, etc.

[0067] The hardware referred to by the names "server," "client," and "service node" in this application is essentially an electronic device with the equivalent capabilities of a personal computer. It is a hardware device with the necessary components revealed by the von Neumann architecture, such as a central processing unit (including an arithmetic logic unit and a control unit), memory, input devices, and output devices. The computer program is stored in its memory, and the central processing unit loads the program stored in the secondary storage into the main memory to run it, execute the instructions in the program, and interact with the input and output devices to complete specific functions.

[0068] It should be noted that the concept of "server" used in this application can also be extended to the case of server clusters. Based on the network deployment principles understood by those skilled in the art, the servers should be logically divided. Physically, these servers can be independent of each other but accessible through interfaces, or they can be integrated into a single physical computer or a computer cluster. Those skilled in the art should understand this flexibility and should not use it to constrain the implementation of the network deployment method in this application.

[0069] One or more of the technical features of this application, unless explicitly specified herein, can be deployed on a server and accessed by a client remotely calling the online service interface provided by the server, or can be directly deployed and run on a client for access.

[0070] Unless otherwise specified, the neural network models referenced or potentially referenced in this application may be deployed on a remote server and invoked remotely on the client, or deployed on a client with the capability to invoke directly. In some embodiments, when running on the client, the corresponding intelligence may be acquired through transfer learning in order to reduce the requirements on the client's hardware resources and avoid excessive consumption of the client's hardware resources.

[0071] Unless otherwise specified, all data involved in this application may be stored remotely on a server or on a local terminal device, as long as it is suitable for use by the technical solution of this application.

[0072] Those skilled in the art will understand that although the various methods in this application are described based on the same concept and thus present commonality among them, they can be performed independently unless otherwise specified. Similarly, the various embodiments disclosed in this application are all based on the same inventive concept; therefore, concepts expressed in the same way, as well as concepts that are appropriately changed for convenience but are expressed differently, should be understood equivalently.

[0073] Unless otherwise expressly stated, the various embodiments disclosed in this application can be combined in a cross-cutting manner to flexibly construct new embodiments, as long as such combination does not depart from the inventive spirit of this application and can meet the needs of the prior art or solve a certain deficiency in the prior art. Those skilled in the art should be aware of such modifications.

[0074] Please see Figure 1 In one embodiment of the robotic arm obstacle avoidance path planning method of this application, the method includes:

[0075] Step S10: Respond to the obstacle avoidance path planning command of the robotic arm and obtain the starting point q of the robotic arm's execution path. start and the current expansion node q expand Construct a system based on the current expansion node q expand A spherical space with center r and radius r is used as the sampling space for random sampling of a preset threshold number of points to determine the random sampling point sequence q. sequence ;

[0076] The harvesting robot can respond to obstacle avoidance path planning instructions from the robotic arm and initialize various parameters, including the starting point q. start , endpoint q goal Sampling step size, bias threshold ε, maximum number of loops N, 3D data of obstacles, and gravitational scale factor K of the gravitational field. att The parameters include: the number of random sampling points (num), the cost function coefficients (k), the bias coefficients (p), the sampling space radius (r0), and the local extremum threshold (m). The starting point (q) is also included. start , endpoint q goal The algorithm employs a three-dimensional coordinate representation in a Cartesian coordinate system. The sampling step size (stepsize) limits the maximum step size for expanding new nodes. The deviation threshold ε indicates that when the Euclidean distance between the newly expanded node and the target point is less than or equal to ε, the target point is considered reached, and the cyclic sampling step is no longer executed. The maximum number of iterations N specifies the maximum number of iterations the algorithm can perform; the algorithm stops after N iterations. The bias coefficient p ranges from 0 to 1. The starting point q... start As the root node of the extended tree T, and with the starting point q set. start To expand node q expand Obtain the starting point q of the robotic arm's execution path. start , endpoint q goal and the current expansion node q expand Construct a system based on the current expansion node q expandLet be the center of a sphere with radius r. This sphere is used as a sampling space for random sampling of a preset threshold number, num. During random sampling in this sampling space, the endpoint q is... goal As one sampling point, the remaining num-1 sampling points are sampled in the sampling space to determine the random sampling point sequence q. sequence .

[0077] Step S20: For the random sampling point sequence q sequence Each random sampling point q in rand The random sampling points are sorted in ascending order of cost using a preset cost function to determine the sorted sequence.

[0078] Random sampling is performed in the sampling space to determine the random sampling point sequence q. sequence Then, for the random sampling point sequence q sequence Each random sampling point q in rand A preset cost function is used to sort the random sampling points from smallest to largest to determine the sorted sequence.

[0079] Furthermore, for the random sampling point sequence q sequence Each random sampling point q in rand The steps for determining the sorted random sampling point sequence by sorting the samples according to their costs from smallest to largest using a preset cost function include:

[0080] The preset cost function is:

[0081] f(q) = g(q) + k·h(q),

[0082] g(q) = cost(q) rand ),

[0083] h(q) = ||q rand -q goal ||,

[0084] Where cost(q) rand ) represents a random sampling point q rand To the starting point q start The path length, ||q rand -q goal ||for q rand and the endpoint q goal The Euclidean distance, where k is the coefficient of the cost function.

[0085] Step S30: Calculate and determine all nodes in the extended tree and the random sampling point q in the sorted random sampling point sequence. randThe Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near ;

[0086] After determining the sorted sequence of random sampling points, calculate the relationship between all nodes in the extended tree and the random sampling points q in the sorted sequence of random sampling points. rand The Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near That is, following the sorted sequence of random sampling points, the relationship between all nodes in the extended tree and random sampling point q is calculated starting from the random sampling point with the lowest cost. rand The Euclidean distance is used to select the node q that is closest to the random sampling point. near .

[0087] Step S40: Based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new ;

[0088] Based on the Euclidean distance, select the node q in the extended tree that is closest to the random sampling point. near Then, based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new ,

[0089] Furthermore, the random sampling point q rand The updated random sampling point q is obtained by moving under the influence of gravity. new_rand The formula for calculating the gravitational force at a random sampling point is:

[0090]

[0091] Among them, U att K represents the gravitational force at random sampling points. att ρ(q) is the scale factor of gravity. rand ,q goal ) represents a random sampling point q rand To the destination q goal Euclidean distance;

[0092] Based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand The calculation formula is as follows:

[0093] q rand =q new_rand +U att ,

[0094] Wherein, the q rand Characterizing random sampling points, the q new_rand The updated random sampling points are represented.

[0095] Furthermore, based on the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new The calculation formula is as follows:

[0096]

[0097] Step S50: Determine the new node q new The node q closest to the random sampling point near The path between them is checked to see if it collides with any obstacles. If no collision occurs, the new node q is moved to the next node. new Add it to the extended tree as a child node of the node closest to the random sampling point, and set the new node q new As the extension node q in the next cycle expand ;

[0098] Determine the new node q new Then, the new node q is determined. new The node q closest to the random sampling point near The path between them is checked to see if it collides with any obstacles. If no collision occurs, the new node q is moved to the next node. new Add it to the extended tree as a child node of the node closest to the random sampling point, and set the new node q new As the extension node q in the next cycle expand .

[0099] Furthermore, determine the new node q. new The node q closest to the random sampling point near The step of detecting whether the path collides with an obstacle includes:

[0100] The new node q was detected. newThe node q closest to the random sampling point near If the path between them collides with an obstacle, the calculation is re-performed to determine all nodes in the extended tree and the random sampling point q in the sorted random sampling point sequence. rand The Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near This step.

[0101] Step S60: Calculate and determine the new node q new The distance between each node in the extended tree, when there exists a node q in the extended tree such that ||q|| new If -q||+cost(q) is minimized, then node q is set to q. new The parent node, where ||q new -q|| represents the Euclidean distance between the two, and cost(q) is the path length from q to the starting point;

[0102] Specifically, the new node q is obtained. new For each node in the extended tree, calculate and determine the new node q. new The distance between each node in the extended tree, when there exists a node q in the extended tree such that ||q|| new If -q||+cost(q) is minimized, then node q is set to q. new The parent node, where ||q new -q|| represents the Euclidean distance between the two, and cost(q) is the path length from q to the starting point;

[0103] Step S70: Determine the new node q new With the extended node q expand Check whether the Euclidean distance between them is less than the local maximum / minimum threshold m, and calculate the number of times it is less than the local maximum / minimum threshold m. When the number is greater than t, adjust the exploration space radius and set the exploration space radius to twice the previous exploration space radius r. After jumping out of the local maximum / minimum region, set the exploration space radius to half of the previous exploration space radius r in turn until it is restored to the original sampling space radius.

[0104] Determine the new node q new With extended node q expand Check if the Euclidean distance between them is less than the local maximum / minimum threshold m, and count the number of times it is less than the local maximum / minimum threshold m. When the number is greater than t, adjust the exploration space radius, setting the exploration space radius r1 to twice the previous exploration space radius r, that is, the exploration space radius r1 = 2 * r.

[0105] After escaping the local maximum / minimum region, the exploration space radius r2 is successively set to half of the previous exploration space radius r, until it is restored to the original sampling space radius r0.

[0106]

[0107] Step S80: Detect the new node q new If the Euclidean distance between the target point and the target point is less than or equal to the deviation threshold ε, then the newly expanded node obtained in the last iteration is taken as the parent node of the endpoint, and the newly expanded node obtained in the last iteration is taken as the child node. The process continues to backtrack through the parent node in the expansion tree until it returns to the starting point q. start ;

[0108] Verify whether the Euclidean distance between the new node and the target point is less than or equal to the deviation threshold ε, i.e., the formula |q new -q goal If |<ε is true, the loop ends; otherwise, continue building the current expanded node q. expand A spherical space with center r and radius r is used as the sampling space for random sampling of a preset threshold number of points to determine the random sampling point sequence q. sequence The next steps.

[0109] Step S90: Sort the parent nodes found by backtracking in a preset order to determine the final path node. Based on the final path node, determine the obstacle avoidance planning path to complete the obstacle avoidance path planning of the robotic arm.

[0110] The newly expanded node q obtained in the last loop expand The parent node that serves as the endpoint, and the newly extended node q obtained from the last loop. expand As a child node, it continuously backtracks to its parent node in the extended tree T until it reaches the root node, i.e., the starting point q. start The final obstacle avoidance planning path is obtained by backtracking and sorting the parent nodes found in the above steps in order, which are the final path nodes. The obstacle avoidance planning path is determined based on the final path nodes to complete the obstacle avoidance path planning of the robotic arm.

[0111] In some embodiments, after the step of sorting the parent nodes found through backtracking in a preset order to determine the final path node, the method includes:

[0112] Step S901: Determine the preset step length of the robotic arm corresponding to the harvesting robot and the initial position of the end effector of the robotic arm;

[0113] Step S903: Based on the final path node and the initial position of the robotic arm's end effector, calculate and determine the motion direction of the robotic arm's end effector;

[0114] Step S905: Calculate the picking position of the robotic arm's end effector based on the initial position of the end effector, the preset step length, and the movement direction of the end effector to complete the obstacle avoidance path planning of the robotic arm.

[0115] As can be seen from the above embodiments, the movement direction of the robotic arm's end effector can be obtained based on the final path node and the initial position of the end effector. Furthermore, the picking position of the end effector can be calculated based on the initial position of the end effector, the preset step length, and the movement direction of the end effector. This enables more efficient path planning and greatly improves the accuracy of obstacle avoidance path planning, laying a solid foundation for the harvesting robot to achieve precise harvesting on farms.

[0116] In some embodiments, the steps of sorting the backtracked parent nodes in a preset order to determine the final path node, and determining the obstacle avoidance planning path based on the final path node to complete the obstacle avoidance path planning of the robotic arm include:

[0117] Step S9001: In response to the fruit picking command, the least squares method is used to fit the final path nodes to generate a smooth obstacle avoidance planning path for the robotic arm.

[0118] Step S9003: The picking robot picks fruit based on the obstacle avoidance planning path of the robotic arm to complete the obstacle avoidance path planning of the robotic arm.

[0119] As can be seen from the above embodiments, compared with the prior art, this application addresses the problems of insufficient flexibility or efficiency in path selection by the robotic arm due to the artificial potential field method, which affects harvesting efficiency, and the high computational complexity, long planning time, and insufficient path quality faced by the RRT method when dealing with dynamic environments and scenarios with high accuracy requirements. This application includes, but is not limited to, the following beneficial effects:

[0120] Firstly, to address the low efficiency of global random exploration in RRT, this application proposes an adaptive spherical sampling space to limit the exploration area and improve search efficiency, while avoiding the problem of getting trapped in local maxima and causing planning failure. To address the slow convergence speed of the sampling algorithm, target gravity is introduced to make the search more directional and improve the convergence speed. This application integrates the cost function in the A* algorithm to further improve search efficiency and convergence speed.

[0121] Secondly, the obstacle avoidance path planning method of the present application reduces the average path length by 16.2%, the planning time by 79.3%, the average number of path nodes by 45.2%, and the number of iterations by 90.5%. The obstacle avoidance path planning method of the present application can effectively improve search efficiency and convergence speed, and can quickly generate high-quality paths.

[0122] Thirdly, the obstacle avoidance path planning method of the robotic arm in this application can greatly improve the applicability and efficiency of the robotic arm in agricultural harvesting operations. By adopting advanced path planning strategies, this application can achieve efficient obstacle avoidance, high-quality path generation, and real-time response to dynamic environments, thereby providing a more reliable and efficient path planning solution for robotic arm harvesting tasks.

[0123] A robotic arm obstacle avoidance path planning device provided to meet one of the purposes of this application includes a sampling point determination module 1100, a sampling point sorting module 1200, a nearest node determination module 1300, a new node determination module 1400, a collision test module 1500, a parent node determination module 1600, an exploration radius adjustment module 1700, a path backtracking module 1800, and an obstacle avoidance path determination module 1900. The sampling point determination module 1100 is configured to respond to the robotic arm obstacle avoidance path planning command and obtain the starting point q of the robotic arm's execution path. start and the current expansion node q expand Construct a system based on the current expansion node q expand A spherical space with center r and radius r is used as the sampling space for random sampling of a preset threshold number of points to determine the random sampling point sequence q. sequence The sampling point sorting module 1200 is configured to sort the random sampling point sequence q. sequence Each random sampling point q in rand A preset cost function is used to sort the random sampling points in ascending order of cost to determine the sorted random sampling point sequence; the nearest node determination module 1300 is configured to calculate and determine the relationship between all nodes in the extended tree and the random sampling points q in the sorted random sampling point sequence. rand The Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near The new node determination module 1400 is configured to determine the node based on the random sampling point q. rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q newCollision test module 1500, configured to determine the new node q new The node q closest to the random sampling point near The path between them is checked to see if it collides with any obstacles. If no collision occurs, the new node q is moved to the next node. new Add it to the extended tree as a child node of the node closest to the random sampling point, and set the new node q new As the extension node q in the next cycle expand The parent node determination module 1600 is configured to calculate and determine the new node q. new The distance between each node in the extended tree, when there exists a node q in the extended tree such that ||q|| new If -q||+cost(q) is minimized, then node q is set to q. new The parent node, where ||q new -q|| represents the Euclidean distance between the two nodes, and cost(q) is the path length from q to the starting point; the exploration radius adjustment module 1700 is set to determine the new node q. new With the extended node q expand The system checks whether the Euclidean distance between the nodes is less than a local extremum threshold m, and counts the number of times the distance is less than the local extremum threshold m. When the number of times is greater than t, the exploration space radius is adjusted by setting it to twice the previous exploration space radius r. After exiting the local extremum region, the exploration space radius is sequentially set to half of the previous exploration space radius r until it is restored to the original sampling space radius. The path backtracking module 1800 is configured to detect the new node q. new If the Euclidean distance between the target point and the target point is less than or equal to the deviation threshold ε, then the newly expanded node obtained in the last iteration is taken as the parent node of the endpoint, and the newly expanded node obtained in the last iteration is taken as the child node. The process continues to backtrack through the parent node in the expansion tree until it returns to the starting point q. start The obstacle avoidance path determination module 1900 is configured to sort the parent nodes found by backtracking in a preset order, determine the final path node, and determine the obstacle avoidance planning path based on the final path node to complete the obstacle avoidance path planning of the robotic arm.

[0124] Based on any embodiment of this application, please refer to Figure 2 Another embodiment of this application also provides an electronic device, which can be implemented by a computer device, such as... Figure 2The diagram shows the internal structure of a computer device. The computer device includes a processor, a computer-readable storage medium, a memory, and a network interface connected via a system bus. The computer-readable storage medium stores an operating system, a database, and computer-readable instructions. The database may store control information sequences. When the processor executes the computer-readable instructions, it enables the processor to implement a robotic arm obstacle avoidance path planning method. The processor provides computational and control capabilities, supporting the operation of the entire computer device. The memory stores computer-readable instructions, which, when executed by the processor, enable the processor to execute the robotic arm obstacle avoidance path planning method of this application. The network interface of the computer device is used for communication with a terminal. Those skilled in the art will understand that… Figure 2 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0125] In this embodiment, the processor executes the specific functions of each module and its sub-modules in the aforementioned robotic arm obstacle avoidance path planning device. The memory stores the program code and various types of data required to execute the aforementioned modules or sub-modules. The network interface is used for data transmission between the user terminal and the server. In this embodiment, the memory stores the program code and data required to execute all modules / sub-modules in the robotic arm obstacle avoidance path planning device of this application, and the server can call the server's program code and data to execute the functions of all sub-modules.

[0126] This application also provides a storage medium storing computer-readable instructions, which, when executed by one or more processors, cause the one or more processors to perform the steps of the robotic arm obstacle avoidance path planning method described in any embodiment of this application.

[0127] This application also provides a computer program product, including a computer program / instructions that, when executed by one or more processors, implement the steps of the robotic arm obstacle avoidance path planning method described in any embodiment of this application.

[0128] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments of this application can be implemented by a computer program instructing related hardware. This computer program can be stored in a computer-readable storage medium, and when executed, it can include the processes of the embodiments of the methods described above. The aforementioned storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0129] The above description is only a partial embodiment of this application. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of this application, and these improvements and modifications should also be considered within the scope of protection of this application.

[0130] In summary, the obstacle avoidance path planning method for robotic arms in this application can greatly improve the applicability and efficiency of robotic arms in agricultural harvesting operations. By adopting advanced path planning strategies, this application can achieve efficient obstacle avoidance, high-quality path generation, and real-time response to dynamic environments, thereby providing a more reliable and efficient path planning solution for robotic arm harvesting tasks.

Claims

1. A method for obstacle avoidance path planning for a robotic arm, characterized in that, include: In response to the barrier avoidance path planning instruction of the mechanical arm, an execution path starting point q of the mechanical arm is acquired start and a current expansion node q expand , a spherical space with the current expansion node q expand as a spherical center and a radius r is constructed, and the spherical space is used as a sampling space for random sampling of a preset number threshold to determine a random sampling point sequence q sequence ; For the random sampling point sequence q sequence Each random sampling point q in rand The random sampling points are sorted in ascending order of cost using a preset cost function to determine the sorted sequence. Calculate and determine all nodes in the extended tree and random sampling points q in the sorted sequence of random sampling points. rand The Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near ; Based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new ; Determine the new node q new The node q closest to the random sampling point near The path between them is checked to see if it collides with any obstacles. If no collision occurs, the new node q is moved to the next node. new Add it to the extended tree as a child node of the node closest to the random sampling point, and set the new node q new As the extension node q in the next cycle expand ; Calculate and determine the new node q new The distance between each node in the extended tree, when there exists a node q in the extended tree such that ||q|| new If -q||+cost(q) is minimized, then node q is set to q. new The parent node, where ||q new -q|| represents the Euclidean distance between the two, and cost(q) is the path length from q to the starting point; Determine the new node q new With the extended node q expand Check whether the Euclidean distance between them is less than the local maximum / minimum threshold m, and calculate the number of times it is less than the local maximum / minimum threshold m. When the number is greater than t, adjust the exploration space radius and set the exploration space radius to twice the previous exploration space radius r. After jumping out of the local maximum / minimum region, set the exploration space radius to half of the previous exploration space radius r in turn until it is restored to the original sampling space radius. Detect the new node q new If the Euclidean distance between the target point and the target point is less than or equal to the deviation threshold ε, then the newly expanded node obtained in the last iteration is taken as the parent node of the endpoint, and the newly expanded node obtained in the last iteration is taken as the child node. The process continues to backtrack through the parent node in the expansion tree until it returns to the starting point q. start ; The parent nodes found through backtracking are sorted in a preset order to determine the final path nodes. Based on the final path nodes, the obstacle avoidance planning path is determined to complete the obstacle avoidance path planning of the robotic arm.

2. The obstacle avoidance path planning method for a robotic arm according to claim 1, characterized in that, For the random sampling point sequence q sequence Each random sampling point q in rand The steps for determining the sorted random sampling point sequence by sorting the samples according to their costs from smallest to largest using a preset cost function include: The preset cost function is: f(q) = g(q) + k·h(q), g(q)=cost(q rand ), h(q)=||q rand -q goal ||, Where cost(q) rand ) represents a random sampling point q rand To the starting point q start The path length, ||q rand -q goal ||for q rand and the endpoint q goal The Euclidean distance, where k is the coefficient of the cost function.

3. The obstacle avoidance path planning method for a robotic arm according to claim 1, characterized in that, Based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new The steps include: The formula for calculating the gravitational force at a random sampling point is: Among them, U att K represents the gravitational force at random sampling points. att ρ(q) is the scale factor of gravity. rand ,q goal ) represents a random sampling point q rand To the destination q goal Euclidean distance; Based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand The calculation formula is as follows: q rand =q new_rand +U att , Wherein, the q rand Characterizing random sampling points, the q new_rand The updated random sampling points are represented.

4. The obstacle avoidance path planning method for a robotic arm according to claim 3, characterized in that, Based on the random sampling point q rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new The steps include: Based on the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new The calculation formula is as follows:

5. The obstacle avoidance path planning method for a robotic arm according to claim 1, characterized in that, Determine the new node q new The node q closest to the random sampling point near The step of detecting whether the path collides with an obstacle includes: The new node q was detected. new The node q closest to the random sampling point near If the path between them collides with an obstacle, the calculation is re-executed to determine all nodes in the extended tree and the random sampling point q in the sorted random sampling point sequence. rand The Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near This step.

6. The obstacle avoidance path planning method for a robotic arm according to claim 1, characterized in that, After sorting the parent nodes found through backtracking in a preset order to determine the final path nodes, the process includes: Determine the preset step length of the robotic arm corresponding to the harvesting robot and the initial position of the end effector of the robotic arm; Based on the final path node and the initial position of the robotic arm's end effector, the motion direction of the robotic arm's end effector is calculated and determined; The picking position of the robotic arm's end effector is calculated based on the initial position of the end effector, the preset step length, and the direction of movement of the end effector, so as to complete the obstacle avoidance path planning of the robotic arm.

7. The obstacle avoidance path planning method for a robotic arm according to any one of claims 1 to 6, characterized in that, The steps of planning the obstacle avoidance path for the robotic arm include: sorting the parent nodes found through backtracking in a preset order to determine the final path nodes; determining the obstacle avoidance path based on the final path nodes; and completing the obstacle avoidance path planning for the robotic arm. In response to the fruit picking command, the least squares method is used to fit the final path nodes to generate a smooth obstacle avoidance planning path for the robotic arm. The harvesting robot harvests fruit based on the obstacle avoidance path planned by the robotic arm, thus completing the obstacle avoidance path planning of the robotic arm.

8. A robotic arm obstacle avoidance path planning device, characterized in that, include: The sampling point determination module is configured to respond to the robotic arm's obstacle avoidance path planning command and obtain the starting point q of the robotic arm's execution path. start and the current expansion node q expand Construct a system based on the current expansion node q expand A spherical space with center r and radius r is used as the sampling space for random sampling of a preset threshold number of points to determine the random sampling point sequence q. sequence ; The sampling point sorting module is configured to sort the random sampling point sequence q. sequence Each random sampling point q in rand The random sampling points are sorted in ascending order of cost using a preset cost function to determine the sorted sequence. The nearest node determination module is configured to calculate and determine the relationship between all nodes in the extended tree and random sampling points q in the sorted sequence of random sampling points. rand The Euclidean distance is used to select the node q in the extended tree that is closest to the random sampling point. near ; The new node determination module is configured to be based on the random sampling point q. rand And the corresponding gravitational calculation determines the updated random sampling point q. new_rand According to the updated random sampling point q new_rand and the node q in the extended tree that is closest to the random sampling point near Calculate and determine the new node q new ; The collision test module is configured to determine the new node q. new The node q closest to the random sampling point near The path between them is checked to see if it collides with any obstacles. If no collision occurs, the new node q is moved to the next node. new Add it to the extended tree as a child node of the node closest to the random sampling point, and set the new node q new As the extension node q in the next cycle expand ; The parent node determination module is configured to calculate and determine the new node q. new The distance between each node in the extended tree, when there exists a node q in the extended tree such that ||q|| new If -q||+cost(q) is minimized, then node q is set to q. new The parent node, where ||q new -q|| represents the Euclidean distance between the two, and cost(q) is the path length from q to the starting point; Explore the radius adjustment module, set it to determine the new node q new With the extended node q expand Check whether the Euclidean distance between them is less than the local maximum / minimum threshold m, and calculate the number of times it is less than the local maximum / minimum threshold m. When the number is greater than t, adjust the exploration space radius and set the exploration space radius to twice the previous exploration space radius r. After jumping out of the local maximum / minimum region, set the exploration space radius to half of the previous exploration space radius r in turn until it is restored to the original sampling space radius. The path backtracking module is configured to detect the new node q. new If the Euclidean distance between the target point and the target point is less than or equal to the deviation threshold ε, then the newly expanded node obtained in the last iteration is taken as the parent node of the endpoint, and the newly expanded node obtained in the last iteration is taken as the child node. The process continues to backtrack through the parent node in the expansion tree until it returns to the starting point q. start ; The obstacle avoidance path determination module is configured to sort the parent nodes found through backtracking in a preset order, determine the final path node, and determine the obstacle avoidance planning path based on the final path node to complete the obstacle avoidance path planning of the robotic arm.

9. An electronic device comprising a central processing unit and a memory, characterized in that, The central processing unit is used to invoke and run a computer program stored in the memory to perform the steps of the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, It stores, in the form of computer-readable instructions, a computer program implemented according to any one of claims 1 to 7, which, when invoked by a computer, executes the steps included in the corresponding method.

Images