Dual-arm cooperative picking path planning method and system based on fixed arm configuration

Abstract

This invention relates to a method and system for planning a collaborative harvesting path using two arms based on the optimal fixed-arm configuration. The method includes: acquiring harvesting task information and constructing an obstacle safety domain; generating multiple candidate fixed-arm configurations with branch fixing points as constraints and selecting feasible sets; constructing fixed-arm obstacle models for each candidate configuration and merging them with the safety domain to obtain a total obstacle set; calculating the collaborative cost of each configuration and selecting the optimal one as the target fixed-arm configuration; planning a collision-free path for the fixed arms and controlling their fixed branches; adding the fixed-arm obstacle models in their fixed state to the harvesting arm obstacle set; and planning a collision-free harvesting path for the harvesting arms. This invention uses quantitative evaluation of collaborative costs to select the optimal fixed-arm configuration, reducing obstruction of the harvesting path and possessing fault-tolerant replanning capabilities, effectively improving the success rate and safety of collaborative harvesting using two arms in complex orchard environments.

Description

Technical Field

[0001] This invention relates to the field of agricultural harvesting robot path planning technology, and in particular to a dual-arm collaborative harvesting path planning method and system based on the optimization of fixed arm configuration. Background Technology

[0002] Apple harvesting is labor-intensive and highly seasonal. The complex environment within orchards, with branches, adjacent fruits, and trunks intertwined, leads to a year-on-year increase in manual harvesting costs. Therefore, apple harvesting robots have become a key research area in the field of intelligent fruit and vegetable equipment.

[0003] Currently, mainstream harvesting robots use a single robotic arm operation mode, relying on visual recognition of the fruit to plan the harvesting path and complete the harvest. However, the complex obstacles in the orchard greatly obstruct the movement of the single arm, and the end of the robotic arm is very likely to scratch nearby apples and pull branches, causing fruit drop and branch damage. This not only greatly reduces the harvesting success rate, but also damages the fruit trees and affects the fruit yield the following year.

[0004] To overcome the inherent drawbacks of single-arm harvesting, the industry has proposed a dual-arm collaborative harvesting solution: one robotic arm (fixed arm) grips and tightens the branches around the target fruit, while the other robotic arm (harvesting arm) performs the harvesting operation, thereby stabilizing the working environment and reducing collision disturbances. However, this solution has revealed new problems in practice—after gripping the branches, the fixed arm itself becomes a static obstacle in the harvesting arm's movement space; if the end effector of the fixed arm is not properly selected, it will directly obstruct the movement channel of the harvesting arm, resulting in an unsolvable path planning problem.

[0005] Most existing dual-arm path planning algorithms are designed for industrial scenarios, focusing on collision avoidance by the arms themselves and path search in static obstacle environments, failing to adapt to the specific working conditions of orchard harvesting. In fact, for the same branch gripping point, the inverse kinematics of the robotic arm can yield multiple feasible fixed-arm configurations, and different configurations have significantly different degrees of occlusion and collision risk on the harvesting path. Currently, existing technologies lack a quantitative collaborative evaluation mechanism for multiple candidate fixed-arm configurations, making it impossible to pre-select the gripping posture with the least occlusion. Furthermore, existing solutions lack fault-tolerant logic; if the selected fixed-arm configuration blocks the harvesting path, the harvesting task must be terminated, resulting in poor operational robustness.

[0006] In summary, how to quantitatively evaluate multiple candidate configurations before the fixed arm grips the branch, select the optimal fixed posture for the low-obstruction target, and provide fault-tolerant replanning logic after path planning failure are urgent problems to be solved in the field of dual-robotic arm apple collaborative harvesting. Summary of the Invention

[0007] This invention provides a dual-arm collaborative harvesting path planning method and system based on the optimized fixed-arm configuration, which solves the technical problems of fixed-arm posture obstructing the harvesting channel, lack of fault tolerance for planning failure, and collision damage to fruit trees in existing dual-arm harvesting systems.

[0008] In a first aspect of the present invention, a dual-arm collaborative harvesting path planning method based on a fixed-arm configuration is provided, comprising: Step S1: Obtain harvesting task information, which includes the fruit to be harvested, branch fixing points, environmental obstacles, and initial configuration information of the dual robotic arms. Step S2: Based on the harvesting task information, construct an obstacle safety domain for the harvesting environment; Step S3: Using the branch fixing point as the end constraint, generate multiple sets of candidate fixing arm configurations for the fixing arm, and perform feasibility screening on each set of candidate fixing arm configurations to obtain a set of feasible candidate configurations. Step S4: For each set of candidate fixed arm configurations in the feasible candidate configuration set, construct a corresponding fixed arm obstacle model, and fuse the fixed arm obstacle model with the obstacle safety domain to obtain the total obstacle set corresponding to each candidate fixed arm configuration. Step S5: Based on the total set of obstacles, calculate the cooperative cost of each candidate fixed arm configuration, and select the candidate fixed arm configuration whose cooperative cost meets the preset conditions as the target fixed arm configuration. Step S6: Starting from the initial configuration of the dual robotic arms and ending at the target fixed arm configuration, a collision-free fixing path for the fixed arm is planned in conjunction with the obstacle safety domain, and the fixed arm is controlled to reach the target fixed arm configuration to fix the branch fixing point. Step S7: Add the fixed arm obstacle model in the fixed state to the obstacle set of the harvesting arm path planning; Step S8: Based on the obstacle set of the picking arm path planning, plan a collision-free picking path for the picking arm from its initial configuration to the picking posture of the fruit to be picked.

[0009] In a second aspect of the present invention, a dual-arm collaborative harvesting path planning system based on a fixed-arm configuration is provided, comprising: The information acquisition module is used to acquire harvesting task information, which includes the fruit to be harvested, the branch fixing point, environmental obstacles, and the initial configuration information of the dual robotic arms. An obstacle modeling module is used to construct an obstacle safety domain for the picking environment based on the picking task information. The candidate configuration generation module is used to generate multiple sets of candidate fixed arm configurations for the fixed arm with the branch fixing point as the end constraint, and to perform feasibility screening on each set of candidate fixed arm configurations to obtain a set of feasible candidate configurations. The fixed arm obstacle conversion module is used to construct a corresponding fixed arm obstacle model for each set of candidate fixed arm configurations in the feasible candidate configuration set, and to fuse the fixed arm obstacle model with the obstacle safety domain to obtain the total obstacle set corresponding to each candidate fixed arm configuration. The cooperative cost evaluation module is used to calculate the cooperative cost of each candidate fixed arm configuration based on the total set of obstacles, and select the candidate fixed arm configuration whose cooperative cost meets the preset conditions as the target fixed arm configuration. The fixed arm path planning module is used to plan a collision-free fixing path for the fixed arm, starting from the initial configuration of the dual robotic arms and ending at the target fixed arm configuration, in conjunction with the obstacle safety domain, and to control the fixed arm to reach the target fixed arm configuration to fix the branch fixing point. The obstacle set update module is used to add the fixed arm obstacle model in a fixed state to the obstacle set of the harvesting arm path planning; The picking arm path planning module is used to plan a collision-free picking path from the initial configuration of the picking arm to the picking posture of the fruit to be picked, based on the obstacle set of the picking arm path planning.

[0010] In a third aspect of the present invention, an electronic device is provided, including a memory, a processor, and a computer program stored in the memory and executable by the processor, wherein the processor executes the computer program to implement the steps of the method as described in the first aspect of the present invention.

[0011] This invention provides a dual-arm collaborative harvesting path planning method and system based on fixed-arm configuration optimization. It generates multiple feasible candidate configurations for the same branch fixing point, and quantitatively evaluates the degree of obstruction, collision risk, and reachability of each configuration to the harvesting channel through a collaborative cost function. It automatically selects the target fixed-arm configuration that minimizes the encroachment on the harvesting arm's movement space, fundamentally reducing the path planning unsolvable problem caused by the fixed arm. When the harvesting arm cannot plan a collision-free path under the selected optimal configuration, it automatically switches to the candidate configuration with the second-best collaborative cost for replanning, avoiding harvesting task interruption due to single configuration failure and significantly improving the success rate of operations in complex orchard environments. Neighboring fruits, branches, trunks, and robotic arm links are all transformed into geometric envelope models of spheres, cylinders, or capsules with safety margins. A unified collision detection standard is maintained throughout the path planning process, effectively preventing the robotic arm from scratching neighboring fruits and pulling branches, reducing fruit drop and damage to the fruit trees. The entire solution features decoupled functional modules, with information acquisition, obstacle modeling, configuration evaluation, and path planning operating independently. It can be flexibly integrated into various six-axis collaborative dual-robotic arm harvesting platforms and is compatible with both simulated offline planning and real-world orchard online operation scenarios. Attached Figure Description

[0012] Figure 1The flowchart of the dual-arm collaborative harvesting path planning method based on fixed arm configuration optimization provided by the present invention is shown below. Figure 2 A schematic diagram of a dual-robotic arm apple harvesting scenario provided by the present invention; Figure 3 A schematic diagram of the safety domain modeling of obstacles in the harvesting environment provided by this invention; Figure 4 This invention provides a flowchart for generating and screening candidate fixed arm configurations. Figure 5 A flowchart for evaluating the collaborative costs of candidate fixed arm configurations provided by this invention; Figure 6 This is a structural block diagram of the dual-arm collaborative harvesting path planning system based on the preferred fixed-arm configuration provided by the present invention. Figure 7 This is a structural block diagram of the electronic device provided by the present invention. Detailed Implementation

[0013] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0014] Figure 1 A flowchart illustrating the dual-arm collaborative harvesting path planning method based on a fixed-arm configuration optimization provided by this invention. (Refer to...) Figure 1 The method includes the following steps: Step S1: Obtain harvesting task information, which includes the fruit to be harvested, branch fixing points, environmental obstacles, and initial configuration information of the dual robotic arms.

[0015] This embodiment uses a dual six-axis collaborative robotic arm harvesting robot in an orchard apple picking scenario as a carrier, referring to... Figure 2 The dual robotic arms consist of a fixed arm and a picking arm, both mounted on the same mobile support platform, with a unified robot coordinate system.

[0016] Specifically, the first step is to collect environmental and robot status information for the harvesting task. This includes: the 3D coordinates of the target apple, the locations of surrounding non-target fruits, the coordinates of the branch clamping points, the spatial outline of the trunk and main branches, the initial joint configuration of the fixed arm and harvesting arm, and the standard harvesting end pose of the target apple. Information can be collected through one or more of the following methods: manual visual calibration, robotic arm teaching point recording, orchard simulation environment preset, and onboard 3D visual perception module. The complete data is stored in the robot's control memory for subsequent modeling and calculations. Figure 2 This is a schematic diagram of a dual-robotic arm apple harvesting scenario provided by the present invention. Figure 2 The target apple, branch anchoring point, anchoring arm path, and harvesting arm path are marked.

[0017] Step S2: Based on the harvesting task information, construct an obstacle safety domain for the harvesting environment.

[0018] Specifically, based on the environmental obstacle distribution information obtained in step S1, a digital model of the environment is established within the computer. To ensure operational safety, this step not only records the physical locations of obstacles but also overlays safety thresholds onto the physical boundaries of the obstacles, generating an expanded obstacle safety domain. This safety domain will serve as a hard constraint in subsequent path planning; any robotic arm pose entering this area will be considered infeasible.

[0019] Step S3: Using the branch fixing point as the end constraint, generate multiple sets of candidate fixing arm configurations, and perform feasibility screening on each set of candidate fixing arm configurations to obtain a set of feasible candidate configurations.

[0020] Among them, the fixed arm configuration refers to the combination of angles of each joint of the fixed arm, which determines the position and attitude of the end effector of the fixed arm in space.

[0021] Specifically, since robotic arms typically have redundant degrees of freedom, for the same branch anchor point, there are often multiple different combinations of joint angles that can all enable the end effector to reach that point. This step generates these possible configurations through calculation and eliminates those configurations that are mathematically achievable but practically impossible to execute, such as those that exceed the physical limits of the joints or that undergo static collisions with the current environment, thereby retaining a set of theoretically executable feasible candidate configurations.

[0022] Step S4: For each set of candidate fixed arm configurations in the feasible candidate configuration set, construct a corresponding fixed arm obstacle model, and fuse the fixed arm obstacle model with the obstacle safety domain to obtain the total obstacle set corresponding to each candidate fixed arm configuration.

[0023] The fixed-arm obstacle model refers to the spatial model occupied by the fixed arm's own links in a specific configuration. Specifically, when the fixed arm is in a particular configuration, its own links occupy a specific volume in space; for a moving harvesting arm, the fixed arm at this time is equivalent to a new dynamic obstacle. This step transforms the fixed arm's own shape in different configurations into an obstacle model and superimposes it onto the environmental obstacle safety domain constructed in step S2. This means that for each candidate configuration, the system maintains a complete list of obstacles including environmental background obstacles and the fixed arm's own obstacles.

[0024] Step S5: Based on the total set of obstacles, calculate the cooperative cost of each candidate fixed arm configuration, and select the candidate fixed arm configuration whose cooperative cost meets the preset conditions as the target fixed arm configuration.

[0025] Specifically, coordination cost refers to an indicator used to quantitatively evaluate the overall impact of a specific fixed-arm configuration on the harvesting task. Its value reflects the unobstructedness of the harvesting path and the operational feasibility under that configuration. The system evaluates the friendliness of each candidate configuration to the harvesting task. For example, while some configurations can hold branches, their arm posture may obstruct the necessary path of the harvesting arm, causing severe blockage; such configurations have high coordination costs. Through quantitative calculations, the set of configurations with the lowest coordination cost is selected from the feasible candidate configuration set and locked as the target fixed-arm configuration for final execution.

[0026] Step S6: Starting from the initial configuration of the dual robotic arms and ending at the target fixed arm configuration, a collision-free fixing path for the fixed arm is planned in conjunction with the obstacle safety domain, and the fixed arm is controlled to reach the target fixed arm configuration to fix the branch fixing point.

[0027] Specifically, after the target is determined, the first step is to plan the movement of the fixed arm itself. While avoiding environmental obstacles, the planner calculates a trajectory that smoothly moves from the current position to the target fixed arm configuration, and drives the fixed arm to perform this movement to achieve a stable grip on the branch.

[0028] Step S7: Add the fixed arm obstacle model in the fixed state to the obstacle set of the picking arm path planning.

[0029] After the fixed arm completes its movement and remains stationary, its spatial shape is officially recognized as the obstacle to be avoided by the harvesting arm. The system updates the obstacle model of the fixed arm to the planning environment of the harvesting arm at this time, ensuring that the harvesting arm can recognize and avoid the fixed arm in the working state during subsequent movements.

[0030] Step S8: Based on the obstacle set of the picking arm path planning, plan a collision-free picking path for the picking arm from its initial configuration to the picking posture of the fruit to be picked.

[0031] Specifically, under complex constraints including environmental obstacles and fixed-arm obstacles, the planner searches for a safe trajectory from the initial position of the picking arm to the fruit position. Since the previous steps have already optimized the fixed-arm configuration that minimizes space occupation, the success rate of planning a collision-free path here will be significantly improved. Finally, the picking arm is controlled to move along this path to complete the picking operation.

[0032] This invention quantifies and evaluates the collaborative costs of each candidate fixed arm configuration before performing fixed arm clamping operations, and selects the target fixed posture that minimizes the encroachment on the picking arm's movement space. This effectively reduces the fixed arm's obstruction of the picking channel and lowers the risk of collision between the two arms and damage to fruit branches. At the same time, the path planning is based on a unified obstacle safety domain for collision detection throughout the process, which significantly improves the success rate and operational safety of dual-arm collaborative picking path planning in complex orchard environments.

[0033] Based on the above embodiments, as an optional implementation, in step S2, constructing an obstacle safety zone for the picking environment based on the picking task information specifically includes: Non-target neighboring fruits in the picking scenario are abstracted into a spherical geometric envelope model, and branches, main branches, and trunks are abstracted into cylindrical or capsule-shaped geometric envelope models. Here, the geometric envelope model refers to a standardized mathematical model obtained by approximating the physical contours of irregular obstacles with regular geometric shapes. Its purpose is to transform complex orchard obstacles into mathematical objects that can be processed by path planning algorithms.

[0034] Based on the actual geometric dimensions of each geometric envelope model, a safety margin is added to generate expanded safety domains for non-target neighboring fruits, main branches, and the trunk, which are then combined to form the obstacle safety domain for the harvesting environment. The safety margin refers to the additional safety distance added outside the actual physical boundary of the obstacle. This distance is used to compensate for positioning errors, motion control errors, and sensor measurement errors of the robotic arm, ensuring that the robotic arm does not make physical contact with the obstacle during movement. The area formed after the safety margin expansion is the obstacle safety domain. This safety domain serves as a hard constraint boundary in path planning; any robotic arm pose entering this domain is deemed infeasible.

[0035] By summarizing the expanded safety zones of non-target neighboring fruits, main branches, and trunks, a set of harvesting environmental obstacles is formed. :

[0036] In the formula, Denotes the set of safe neighbor regions for non-target results. This represents the set of safe regions for branches and main branches. This represents a set of obstacles on the tree trunk.

[0037] Figure 3 This is a schematic diagram illustrating the safety domain modeling of obstacles in the harvesting environment provided by the present invention. (See diagram below.) Figure 3 As shown in the figure, this diagram visually illustrates the geometric expansion safety domain modeling method for various fruit tree obstacles. The solid lines in the diagram represent the actual physical boundaries of the fruit trees, while the dashed lines represent the safety domain boundaries generated after increasing the safety margin. Specifically, for non-target neighboring fruits surrounding the target apple, a sphere model is used to enclose and expand outwards to form a safety domain for non-target neighboring fruits; for slender branches and thicker main branches, a cylinder or capsule model is used for fitting to generate branch safety domains and main branch safety domains; similarly, a cylinder or capsule model is used to enclose the trunk at the bottom to form a trunk safety domain. This modeling method transforms complex natural forms into regular mathematical geometries, providing a unified data foundation for the subsequent fusion calculation in step S4 and simplifying the complexity of the collision detection algorithm.

[0038] This embodiment selects an appropriate geometric envelope model for complex objects in orchards. By increasing the safety margin, it generates various obstacle safety domains, which can adapt to the irregular shape of fruit trees. It reserves safe operation gaps in path planning, reduces the probability of collision damage between the robotic arm and fruit tree branches and fruits, and improves the reliability and real-time performance of collision detection in orchard scenarios.

[0039] Based on the above embodiments, as an optional implementation, in step S3, multiple sets of candidate fixed arm configurations are generated using the branch fixing point as the end constraint, and the feasibility of each set of candidate fixed arm configurations is screened to obtain a set of feasible candidate configurations, specifically including: Using the branch fixing point as the end constraint, multiple sets of candidate fixed arm configurations are generated by at least one of inverse kinematics solution, end clamping posture perturbation and joint space sampling. For each group of candidate fixed arm configurations, the following feasibility tests were performed sequentially: Joint limit detection eliminates candidate fixed arm configurations where any joint exceeds the positive or negative limit angle; End-effector attitude detection eliminates candidate fixed arm configurations whose gripper orientation does not meet the clamping requirements; Environmental collision detection eliminates candidate fixed arm configurations that may collide with obstacles in the harvesting environment; Candidate fixed arm configurations that pass all feasibility tests are included in the set of feasible candidate configurations.

[0040] Among them, joint limit detection is used to verify whether the rotation angle of each joint is within the positive and negative limit rotation angle range set by the robot arm at the factory; end posture detection is used to verify whether the orientation of the fixed arm gripper can complete the clamping operation of the branch fixing point; environmental collision detection is used to verify whether the fixed arm link will have spatial interference collision with obstacles in the picking environment under the candidate fixed arm configuration.

[0041] Figure 4 This is a flowchart illustrating the generation and feasibility screening process for candidate fixed arm configurations provided by the present invention. Figure 4 As shown in the figure, the complete process is illustrated from the input of the branch fixing point and the posture requirements of the fixed arm end to the final output of a set of feasible candidate configurations. Figure 4 The upper middle part is the candidate configuration generation stage: taking the branch fixation point and end posture requirements as input, multiple candidate fixed arm configurations are generated through one or more combinations of three approaches: inverse kinematics solution, end clamping posture perturbation, and joint space sampling; in the attached figure, "candidate fixed arm configuration" is simply referred to as "candidate configuration" or "candidate fixed configuration", which have the same meaning. Figure 4 The lower part is the feasibility screening stage: for each generated candidate configuration, joint limit detection, end pose detection and environmental collision detection are performed in sequence. If any one of the three detections fails, the candidate configuration is determined to be infeasible and is eliminated; the candidate configuration that passes all three detections is included in the set of feasible candidate configurations. Figure 4 This clearly demonstrates the technical means used in this embodiment to perform efficient search and strict constraint filtering of the configuration space.

[0042] Specifically, considering the redundancy of robotic arm motion, a single inverse kinematics solution may not satisfy all working conditions. This step employs a hybrid generation strategy: first, the basic configuration is solved using inverse kinematics algorithms; second, the end-effector gripping posture is slightly perturbed (e.g., a small-angle rotational offset is applied around the branch gripping axis) to explore more possible approach directions; finally, uniform sampling in the joint space is combined to cover potential feasible solutions near the singular points of the inverse solution. Through the above methods, an initial candidate pool containing multiple posture possibilities is constructed.

[0043] This embodiment generates multiple candidate configurations by combining inverse kinematics solution, end-effector posture perturbation, and joint space sampling. It also uses triple detection of joint constraints, end-effector posture, and environmental collision to screen feasible configurations. This approach can fully utilize the redundant degrees of freedom of the robotic arm to obtain sufficient effective candidates, while ensuring the physical realizability and environmental safety of the candidate configurations, thus providing a reliable data foundation for subsequent collaborative cost assessment.

[0044] Based on the above embodiments, as an optional implementation, in step S4, for each group of candidate fixed-arm configurations in the feasible candidate configuration set, a corresponding fixed-arm obstacle model is constructed, and the fixed-arm obstacle model is fused with the obstacle safety domain to obtain the total obstacle set corresponding to each candidate fixed-arm configuration, specifically including: For each set of candidate fixed arm configurations in the feasible candidate configuration set, the spatial coordinates of the joints at both ends of each link in the robot base coordinate system are solved by the forward kinematics of the fixed arm. Combined with the preset link envelope radius, each link of the fixed arm is enveloped by a capsule model. The capsule models corresponding to all links are summarized to obtain the fixed arm obstacle model corresponding to this set of candidate fixed arm configurations.

[0045] Specifically, for the first k Candidate fixed arm configurations The coordinates of each joint of the fixed arm in the robot's base coordinate system are calculated using the forward kinematics equations. Based on this, the envelope radius of each link is preset, and a capsule-shaped geometric model is constructed to approximate the envelope of each link of the fixed arm. The fixed arm's first... i Capsule model with connecting rods It can be represented as:

[0046] In the formula, and The fixed arm is the first i Spatial coordinates of the joints at both ends of the link. Let be the radius of the link's envelope. This yields the candidate fixed configuration. Fixed-arm obstacle model .

[0047] Furthermore, the fixed-arm obstacle model is fused with the obstacle safety domain to obtain the total obstacle set corresponding to the candidate fixed-arm configurations. Specifically, the fixed-arm obstacle model calculated above is... With the set of harvesting environmental obstacles constructed in step S2 Perform a union operation to obtain the total set of candidate obstacles. :

[0048] The total obstacle set fully describes all spatial constraints faced by the harvesting arm under a specific fixed arm configuration, including environmental background obstacles (such as tree trunks and neighboring fruits) and obstacles of the fixed arm itself.

[0049] This embodiment transforms each link of the fixed arm into a standardized obstacle model through forward kinematics solution and capsule envelope, and merges it with the environmental obstacle safety domain to form the total obstacle set corresponding to each candidate configuration. This allows subsequent path planning to consider both environmental obstacles and fixed arm obstacles within a unified collision detection framework, eliminating the need to design separate collision detection strategies for different types of obstacles. This effectively simplifies the collision detection calculation process and improves the efficiency and reliability of path search.

[0050] Based on the above embodiments, as an optional implementation, in step S5, the cooperative cost of each candidate fixed arm configuration is calculated based on the total set of obstacles, and the candidate fixed arm configuration whose cooperative cost meets the preset conditions is selected as the target fixed arm configuration, specifically including: For each candidate fixed arm configuration in the set of feasible candidate configurations, the cooperative cost is calculated according to the following cooperative cost function:

[0051] In the formula, Indicates the first feasible candidate configuration within the set. k Candidate fixed arm configurations for the group of fixed arms, subscript m Represents the fixed arm, superscript k Candidate configuration number; Indicates candidate fixed arm configuration The cost of collaboration; This indicates the cost of obstructing the harvesting route. This indicates the cost of environmental collision risks. This indicates the cost of accessibility to the harvesting arm. This represents the cost of fixed-arm movement. w 1. w 2. w 3 and w 4 represents the weighting coefficients for each cost item; Among them, the cost of blocked picking channels This is used to measure the degree to which candidate fixed-arm configurations obstruct the pre-harvesting movement channel of the harvesting arm. Specifically, the harvesting channel obstruction cost is determined based on the minimum distance between the sampling point of the pre-harvesting channel of the harvesting arm and the obstacle model of the fixed arm. The smaller the distance between the sampling point and the obstacle model of the fixed arm, the higher the harvesting channel obstruction cost, indicating that the configuration hinders the movement of the harvesting arm more severely.

[0052] Environmental collision risk costs This is used to measure the degree of collision risk between the fixed arm and the safety zone of obstacles in the harvesting environment under the candidate fixed arm configuration.

[0053] Accessibility Costs of Harvesting Arms This is used to measure the ease with which the harvesting arm completes the harvesting task in this fixed-arm configuration. Specifically, the reachability cost of the harvesting arm is determined based on at least one of the following: the number of feasible inverse kinematic solutions for the harvesting arm to reach the harvesting pose of the fruit to be harvested, the number of planned collision-free paths, and the attitude deviation of the harvesting arm's end effector. The more feasible solutions, the smoother the paths, and the smaller the attitude deviation, the lower the cost.

[0054] Fixed arm motion cost Used to measure motion losses such as path length or energy consumption of the fixed arm from the initial configuration to the candidate fixed arm configuration.

[0055] The four-dimensional cooperative cost function described above is used to iterate through and evaluate each set of candidate configurations in the feasible candidate configuration set, and the cooperative cost corresponding to each candidate configuration is recorded. In this application, "feasible candidate configuration" is an abbreviation for "feasible candidate fixed arm configuration". In the accompanying drawings, "target fixed arm configuration" is abbreviated as "target configuration" or "target fixed configuration", which have the same meaning. Figure 5 A flowchart illustrating the collaborative cost evaluation process for candidate fixed-arm configurations provided by this invention. Figure 5 As shown in the figure, this diagram fully illustrates the complete evaluation process from inputting a set of candidate fixed-arm configurations to outputting the target fixed-arm configuration. The diagram first uses the set of candidate fixed-arm configurations as input. For each set of candidate configurations, the costs of harvesting channel obstruction, environmental collision risk, harvesting arm accessibility, and fixed-arm movement are calculated in parallel or sequentially. These costs are then weighted and calculated to obtain the cooperative cost of the candidate configuration, which is recorded. After all candidate configurations have been calculated, the candidate configuration with the lowest cooperative cost is selected as the target fixed-arm configuration output. This process ensures that the finally selected target fixed-arm configuration can both secure the branches and minimize the obstruction of the harvesting arm's subsequent movement path, while simultaneously considering environmental safety and operational efficiency.

[0056] After traversing and calculating from the set of feasible candidate configurations, a collaborative cost is selected. The smallest candidate fixed arm configuration is selected as the target fixed arm configuration. .

[0057]

[0058] In the formula, This represents the set of feasible candidate configurations.

[0059] Based on the above embodiments, as an optional implementation, in step S8, based on the obstacle set of the picking arm path planning, a collision-free picking path is planned from the initial configuration of the picking arm to the picking pose of the fruit to be picked, specifically including: With the fixed arm locked, the capsule-shaped obstacle model of the fixed arm is incorporated into the global obstacle set as a constraint for the harvesting arm path planning. Starting from the initial configuration of the harvesting arm and ending at the harvesting pose of the fruit, a sampling-based path planning algorithm or a graph search algorithm is used for path searching.

[0060] Among them, sampling path planning algorithm refers to the algorithm that constructs a path in the configuration space by random or pseudo-random sampling, while graph search algorithm refers to the algorithm that finds a path in the discretized state space or configuration space by searching the connectivity between nodes.

[0061] Specifically, the sampling path planning algorithm can be RRT (Rapidly-exploring Random Tree) or RRT... (Rapidly-exploring Random Tree Star, asymptotically optimal rapid expanding random tree), Bi-RRT (Bidirectional Rapidly-exploring Random Tree), PRM (Probabilistic Roadmap), or improved algorithms thereof. The graph search algorithm can be A... (A-star) or its improved algorithm. During the planning process, the fixed arm obstacle model in the fixed state determined in step S7 is incorporated into the global obstacle set as a dynamic constraint. When the algorithm performs random sampling or node expansion in the configuration space, it detects in real time whether the sampled node and the path edge connecting the node collide with obstacles in the picking environment (such as non-target neighboring fruits, branches, trunks, main branches) or the fixed arm obstacle model, or whether it enters a preset danger zone. If a collision or entry into a danger zone is detected, the corresponding sampled node or path edge is determined to be infeasible and discarded from the search tree or roadmap; only nodes and path edges that meet the no-collision condition are allowed to be added to the path search results until a safe trajectory from the initial configuration of the picking arm to the picking pose of the fruit to be picked is planned.

[0062] Furthermore, a replanning mechanism based on cooperative cost ranking is established. If, under the current target fixed-arm configuration, the harvesting arm cannot generate an effective collision-free harvesting path (e.g., due to severe occlusion of the fixed-arm posture causing search timeout or no solution), then the current configuration is determined to be theoretically the lowest cost but practically unexecutable. In this case, the feasible candidate configuration set is returned, and the candidate configuration with the second-best cooperative cost is selected as the new target fixed-arm configuration. Steps S6 to S8 are repeated: that is, the fixed path of the fixed arm from the initial configuration to the new target fixed-arm configuration is replanned, the fixed arm is controlled to reach the new position and fix the branch, the obstacle set for the harvesting arm path planning is updated, and the collision-free harvesting path of the harvesting arm is planned again. The above process is executed cyclically until a collision-free path of both arms is successfully planned, or if a successful planning is still not achieved after traversing all feasible candidate configurations, then the harvesting task is determined to be unexecutable and an error is reported.

[0063] This embodiment effectively solves the local deadlock problem that may be caused by a single optimal configuration by strictly implementing collision detection in path planning and introducing a fault-tolerant replanning mechanism based on cooperative cost; when the preferred configuration is blocked, it can automatically switch to the suboptimal configuration for retry, which significantly improves the success rate and robustness of dual-arm collaborative harvesting tasks in complex unstructured environments.

[0064] Based on the above embodiments, as an optional implementation, after step S8, the method provided by the present invention further includes: Step S9: Path optimization and harvesting execution.

[0065] Redundant nodes are deleted, paths are smoothed, and time parameters are applied to the collision-free fixing path of the fixed arm and the collision-free picking path of the picking arm to generate a dual-arm collaborative picking trajectory. The fixed arm is controlled to move along the optimized collaborative picking trajectory to the target fixed arm configuration to fix the branch fixing point, and then the picking arm is controlled to move along the optimized collaborative picking trajectory to the position of the fruit to be picked to complete the picking operation.

[0066] Specifically, the collision-free fixed path of the fixed arm planned in step S6 and the collision-free picking path of the picking arm planned in step S8 are both discrete configuration sequences, which may contain a large number of collinear or nearly collinear redundant intermediate nodes. First, redundant nodes are removed by sequentially checking whether three adjacent nodes in the path are nearly collinear; if so, the intermediate nodes are removed to reduce the number of trajectory points and lower the computational burden of subsequent control. Then, path smoothing is performed, using spline interpolation or Bézier curve fitting to create a continuous transition between discrete path points, eliminating abrupt changes in the broken-line trajectory caused by random sampling, making the joint movements of the robotic arm smoother and more fluid. Finally, time parameterization is performed, assigning a timestamp to each discrete point on the path based on the maximum allowable speed and acceleration constraints of each joint, generating a complete executable trajectory containing position, attitude, and time information. These three processes are applied sequentially to the fixed arm fixed path and the picking arm picking path, ultimately generating the executable collaborative picking trajectory for the fixed arm and the picking arm, respectively.

[0067] During the dual-arm coordinated execution phase, strict timing constraints are followed: First, the fixed arm is driven to move along its corresponding executable coordinated picking trajectory until the end effector reaches the target fixed arm configuration and securely clamps the branch at the fixing point, keeping the branch stationary during subsequent picking. Then, with the fixed arm locked, the picking arm is driven to move along its corresponding executable coordinated picking trajectory, causing the end effector of the picking arm to gradually approach the fruit to be picked. Once the end effector of the picking arm reaches the picking posture of the fruit, a stable and damage-free apple picking operation is completed through clamping, rotation, pulling, shearing, or other methods. Throughout the entire execution process, the fixed arm and the picking arm operate in a strictly time-separated manner, avoiding the risk of interference and collision that might occur if both arms move simultaneously.

[0068] This embodiment transforms a discrete path into a continuous and executable collaborative harvesting trajectory by deleting redundant nodes, smoothing the path, and parameterizing the time, thus ensuring the stability and controllability of the robotic arm's movement. At the same time, the time-sharing execution strategy of fixing the fixed arm first and harvesting the harvesting arm later effectively avoids the risk of interference and collision caused by the synchronous movement of the two arms, ensuring the safety and reliability of the collaborative harvesting operation.

[0069] Figure 6 The overall architecture diagram of the dual-arm collaborative harvesting path planning system based on the optimized fixed-arm configuration provided by the present invention is shown below. Figure 6 The system includes: The information acquisition module is used to acquire harvesting task information, which includes the fruit to be harvested, the branch fixing point, environmental obstacles, and the initial configuration information of the dual robotic arms. An obstacle modeling module is used to construct an obstacle safety domain for the picking environment based on the picking task information. The candidate configuration generation module is used to generate multiple sets of candidate fixed arm configurations for the fixed arm with the branch fixing point as the end constraint, and to perform feasibility screening on each set of candidate fixed arm configurations to obtain a set of feasible candidate configurations. The fixed arm obstacle conversion module is used to construct a corresponding fixed arm obstacle model for each set of candidate fixed arm configurations in the feasible candidate configuration set, and to fuse the fixed arm obstacle model with the obstacle safety domain to obtain the total obstacle set corresponding to each candidate fixed arm configuration. The cooperative cost evaluation module is used to calculate the cooperative cost of each candidate fixed arm configuration based on the total set of obstacles, and select the candidate fixed arm configuration whose cooperative cost meets the preset conditions as the target fixed arm configuration. The fixed arm path planning module is used to plan a collision-free fixing path for the fixed arm, starting from the initial configuration of the dual robotic arms and ending at the target fixed arm configuration, in conjunction with the obstacle safety domain, and to control the fixed arm to reach the target fixed arm configuration to fix the branch fixing point. The obstacle set update module is used to add the fixed arm obstacle model in a fixed state to the obstacle set of the harvesting arm path planning; The picking arm path planning module is used to plan a collision-free picking path from the initial configuration of the picking arm to the picking posture of the fruit to be picked, based on the obstacle set of the picking arm path planning.

[0070] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working process of the above system and modules can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0071] Figure 7 A structural block diagram of the electronic device provided by the present invention, such as Figure 7 As shown, the present invention also provides an electronic device 700, which can be a mobile terminal, desktop computer, laptop, handheld computer, server, or other computing device. The electronic device 700 includes a processor 701 and a memory 702, wherein the memory 702 stores a dual-arm cooperative harvesting path planning program 703 based on a fixed-arm configuration. In one embodiment, when the dual-arm cooperative harvesting path planning program 703 based on a fixed-arm configuration is executed by the processor 701, it performs the following steps: Acquire harvesting task information, which includes the fruit to be harvested, branch fixing points, environmental obstacles, and initial configuration information of the dual robotic arms; Based on the harvesting task information, an obstacle safety domain is constructed for the harvesting environment; Using the branch fixing point as the end constraint, multiple sets of candidate fixing arm configurations are generated, and the feasibility of each set of candidate fixing arm configurations is screened to obtain a set of feasible candidate configurations. For each set of candidate fixed arm configurations in the set of feasible candidate configurations, a corresponding fixed arm obstacle model is constructed, and the fixed arm obstacle model is fused with the obstacle safety domain to obtain the total obstacle set corresponding to each candidate fixed arm configuration. Based on the total set of obstacles, the cooperative cost of each candidate fixed arm configuration is calculated, and the candidate fixed arm configuration whose cooperative cost meets the preset conditions is selected as the target fixed arm configuration. Starting from the initial configuration of the dual robotic arms and ending at the target fixed arm configuration, a collision-free fixing path for the fixed arm is planned in conjunction with the obstacle safety domain, and the fixed arm is controlled to reach the target fixed arm configuration to fix the branch fixing point. The fixed arm obstacle model in a fixed state is added to the obstacle set of the harvesting arm path planning; Based on the obstacle set of the picking arm path planning, a collision-free picking path is planned from the initial configuration of the picking arm to the picking posture of the fruit to be picked.

[0072] This embodiment also provides a computer-readable storage medium storing a dual-arm cooperative harvesting path planning program based on a fixed-arm configuration. When the dual-arm cooperative harvesting path planning program based on a fixed-arm configuration is executed by a processor, it implements the steps of the dual-arm cooperative harvesting path planning method based on a fixed-arm configuration.

[0073] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

[0074] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A dual-arm collaborative harvesting path planning method based on fixed-arm configuration optimization, characterized in that, include: Step S1: Obtain harvesting task information, which includes the fruit to be harvested, branch fixing points, environmental obstacles, and initial configuration information of the dual robotic arms. Step S2: Based on the harvesting task information, construct an obstacle safety domain for the harvesting environment; Step S3: Using the branch fixing point as the end constraint, generate multiple sets of candidate fixing arm configurations for the fixing arm, and perform feasibility screening on each set of candidate fixing arm configurations to obtain a set of feasible candidate configurations. Step S4: For each set of candidate fixed arm configurations in the feasible candidate configuration set, construct a corresponding fixed arm obstacle model, and fuse the fixed arm obstacle model with the obstacle safety domain to obtain the total obstacle set corresponding to each candidate fixed arm configuration. Step S5: Based on the total set of obstacles, calculate the cooperative cost of each candidate fixed arm configuration, and select the candidate fixed arm configuration whose cooperative cost meets the preset conditions as the target fixed arm configuration. Step S6: Starting from the initial configuration of the dual robotic arms and ending at the target fixed arm configuration, a collision-free fixing path for the fixed arm is planned in conjunction with the obstacle safety domain, and the fixed arm is controlled to reach the target fixed arm configuration to fix the branch fixing point. Step S7: Add the fixed arm obstacle model in the fixed state to the obstacle set of the harvesting arm path planning; Step S8: Based on the obstacle set of the picking arm path planning, plan a collision-free picking path for the picking arm from its initial configuration to the picking posture of the fruit to be picked.

2. The dual-arm collaborative harvesting path planning method based on fixed-arm configuration optimization according to claim 1, characterized in that, In step S2, based on the harvesting task information, an obstacle safety domain for the harvesting environment is constructed, specifically including: Non-target neighboring fruits are abstracted into spherical geometric envelope models, and branches, main branches, and trunks are abstracted into cylindrical or capsule geometric envelope models. Based on the actual geometric dimensions of each geometric envelope model, a safety margin is added to generate an expanded non-target neighboring fruit safety domain, a branch main branch safety domain, and a trunk safety domain, which are then combined to form the harvesting environment obstacle safety domain.

3. The dual-arm collaborative harvesting path planning method based on fixed-arm configuration optimization according to claim 1, characterized in that, In step S3, using the branch fixing point as the end constraint, multiple sets of candidate fixing arm configurations are generated, and the feasibility of each set of candidate fixing arm configurations is screened to obtain a set of feasible candidate configurations, specifically including: Using the branch fixing point as the end constraint, multiple sets of candidate fixed arm configurations are generated by at least one of inverse kinematics solution, end clamping posture perturbation and joint space sampling. For each group of candidate fixed arm configurations, the following feasibility tests were performed sequentially: Joint limit detection eliminates candidate fixed arm configurations where any joint exceeds the positive or negative limit angle; End-effector attitude detection eliminates candidate fixed arm configurations whose gripper orientation does not meet the clamping requirements; Environmental collision detection eliminates candidate fixed arm configurations that may collide with obstacles in the harvesting environment; Candidate fixed arm configurations that pass all feasibility tests are included in the set of feasible candidate configurations.

4. The dual-arm collaborative harvesting path planning method based on fixed arm configuration optimization according to claim 1, characterized in that, In step S4, for each group of candidate fixed-arm configurations in the feasible candidate configuration set, a corresponding fixed-arm obstacle model is constructed, and the fixed-arm obstacle model is fused with the obstacle safety domain to obtain the total obstacle set corresponding to each candidate fixed-arm configuration, specifically including: For each set of candidate fixed arm configurations in the set of feasible candidate configurations, the spatial coordinates of the joints at both ends of each link in the robot base coordinate system are solved by the forward kinematics of the fixed arm. Combined with the preset link envelope radius, each link of the fixed arm is enveloped by a capsule model. The capsule models corresponding to all links are summarized to obtain the fixed arm obstacle model corresponding to this set of candidate fixed arm configurations. By fusing the fixed-arm obstacle model with the obstacle safety domain, the total obstacle set corresponding to the candidate fixed-arm configurations in this group is obtained.

5. The dual-arm collaborative harvesting path planning method based on fixed-arm configuration optimization according to claim 1, characterized in that, In step S5, based on the total set of obstacles, the cooperative cost of each candidate fixed arm configuration is calculated, and the candidate fixed arm configuration whose cooperative cost meets the preset conditions is selected as the target fixed arm configuration. Specifically, this includes: For each candidate fixed arm configuration in the set of feasible candidate configurations, the cooperative cost is calculated according to the following cooperative cost function: In the formula, Indicates the first feasible candidate configuration within the set. k Candidate fixed arm configurations for the group of fixed arms, subscript m Represents the fixed arm, superscript k Candidate configuration number; Indicates candidate fixed arm configuration The cost of collaboration; This indicates the cost of obstructing the harvesting route. This indicates the cost of environmental collision risks. This indicates the cost of accessibility to the harvesting arm. Indicates the cost of fixed arm movement; w 1. w 2. w 3 and w 4 represents the weighting coefficients for each cost item; The candidate fixed arm configuration with the minimum cooperative cost is selected from the set of feasible candidate configurations as the target fixed arm configuration.

6. The dual-arm collaborative harvesting path planning method based on fixed-arm configuration optimization according to claim 5, characterized in that, The cost of blocking the picking channel is determined based on the minimum distance between the sampling point of the picking arm pre-picking channel and the obstacle model of the fixed arm. The smaller the distance between the sampling point and the obstacle model of the fixed arm, the higher the value of the cost of blocking the picking channel. The reachability cost of the picking arm is determined based on at least one of the following: the number of feasible inverse kinematic solutions for the picking arm to reach the picking pose of the fruit to be picked, the number of feasible paths, and the end-effector posture deviation of the picking arm.

7. The dual-arm collaborative harvesting path planning method based on fixed arm configuration optimization according to claim 1, characterized in that, In step S8, based on the obstacle set of the harvesting arm path planning, a collision-free harvesting path is planned from the initial configuration of the harvesting arm to the harvesting pose of the fruit to be harvested, specifically including: A sampling-based path planning algorithm or a graph search algorithm is used to perform collision detection on sampling nodes and path edges, and sampling nodes and path edges that interfere with each other are discarded. If the collision-free picking path planning of the picking arm fails under the current target fixed arm configuration, return to the set of feasible candidate configurations, select the candidate configuration with the second-best cooperative cost as the new target fixed arm configuration, repeat steps S6 to S8, replan the fixed path of the fixed arm and the picking path of the picking arm, until the planning is successful or all feasible candidate configurations are traversed.

8. The dual-arm collaborative harvesting path planning method based on fixed arm configuration optimization according to claim 1, characterized in that, Also includes: Redundant nodes are deleted, paths are smoothed, and time parameters are applied to the collision-free fixed path of the fixed arm and the collision-free picking path of the picking arm to generate a dual-arm collaborative picking trajectory. The control arm first moves along the optimized collaborative picking trajectory to the target control arm configuration to fix the branch fixing point, and then controls the picking arm to move along the optimized collaborative picking trajectory to the position of the fruit to be picked to complete the picking operation.

9. A dual-arm collaborative harvesting path planning system based on a fixed-arm configuration, the system being used to execute the dual-arm collaborative harvesting path planning method based on a fixed-arm configuration as described in any one of claims 1-8, characterized in that, The system includes: The information acquisition module is used to acquire harvesting task information, which includes the fruit to be harvested, the branch fixing point, environmental obstacles, and the initial configuration information of the dual robotic arms. An obstacle modeling module is used to construct an obstacle safety domain for the picking environment based on the picking task information. The candidate configuration generation module is used to generate multiple sets of candidate fixed arm configurations for the fixed arm with the branch fixing point as the end constraint, and to perform feasibility screening on each set of candidate fixed arm configurations to obtain a set of feasible candidate configurations. The fixed arm obstacle conversion module is used to construct a corresponding fixed arm obstacle model for each set of candidate fixed arm configurations in the feasible candidate configuration set, and to fuse the fixed arm obstacle model with the obstacle safety domain to obtain the total obstacle set corresponding to each candidate fixed arm configuration. The cooperative cost evaluation module is used to calculate the cooperative cost of each candidate fixed arm configuration based on the total set of obstacles, and select the candidate fixed arm configuration whose cooperative cost meets the preset conditions as the target fixed arm configuration. The fixed arm path planning module is used to plan a collision-free fixing path for the fixed arm, starting from the initial configuration of the dual robotic arms and ending at the target fixed arm configuration, in conjunction with the obstacle safety domain, and to control the fixed arm to reach the target fixed arm configuration to fix the branch fixing point. The obstacle set update module is used to add the fixed arm obstacle model in a fixed state to the obstacle set of the harvesting arm path planning; The picking arm path planning module is used to plan a collision-free picking path from the initial configuration of the picking arm to the picking posture of the fruit to be picked, based on the obstacle set of the picking arm path planning.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the dual-arm collaborative harvesting path planning method based on the preferred fixed-arm configuration as described in any one of claims 1 to 8.

Images