Apple picking system with double six-degree-of-freedom variable working space mechanical arms and control method

Abstract

The invention discloses a double-six-degree-of-freedom variable working space mechanical arm apple picking system and a picking control method, and belongs to agricultural automation equipment. Aiming at the problems of low picking coverage rate and low efficiency caused by insufficient working space rigid constraint and dynamic precision of an existing apple picking robot, the system comprises a mobile platform, two six-degree-of-freedom mechanical arms, an independent shear type lifting unit, an end effector and an integrated control system, the shear type lifting unit and a servo motor drive lead screw achieve single-arm vertical lifting through an independent X-shaped cross hinge mechanism, the double-arm cooperative control module distributes fruit picking tasks based on a space-time benefit cost function, and the dynamic vision calibration module conducts double-thread parallel processing through a laser range finder and a vision sensor to correct the coordinate mapping relation in real time. The double-arm synchronous lifting rigid constraint is broken through, independent height adjustment is achieved, the crown fruit coverage rate is increased, the positioning error is reduced, single fruit picking time is shortened, the picking efficiency is improved, the branch collision risk is reduced, and the device is suitable for the complex environment.

Description

Technical Field

[0001] This invention relates to the field of agricultural automation equipment technology, and more particularly to apple harvesting technology. Background Technology

[0002] Apple harvesting is a labor-intensive part of orchard production. Traditional manual harvesting suffers from low efficiency (approximately 200-300 kg per person per day), high costs (accounting for 35%-40% of production costs), and high labor intensity (requiring frequent climbing and bending). With the increasing shortage of agricultural labor and the demand for intelligent agriculture, apple harvesting robot technology has become a core solution to replace manual harvesting. Its research and development goal is to achieve efficient, low-loss, and adaptable fruit harvesting in complex orchard environments through automated equipment.

[0003] I. Existing technical solutions and core defects.

[0004] Based on the technological development path, existing apple harvesting robots can be mainly divided into the following types, but all of them have significant limitations:

[0005] (a) Single-arm robotic arm harvesting robot.

[0006] Technical features: It consists of a mobile platform, a single six-degree-of-freedom robotic arm, an end effector, and a vision system. It uses vision to locate the fruit and controls the robotic arm to complete the grasping, cutting, and placing actions.

[0007] Its limitations include:

[0008] 1. Fixed workspace: The range of motion of the robotic arm is limited by the position of the base and the angle of joint movement, making it unable to adapt to the complex tree morphology in orchards (different varieties of apple trees vary in height by 2-4m, and the crown shape is divided into sparse layered, spindle-shaped, etc.) and planting environment (hilly terrain causes fruit trees to tilt and obstacles between rows to block the view). For example, a single robotic arm has difficulty reaching the inside of the canopy (30-50cm from the trunk) or fruits that are more than 2.5m tall, resulting in a low harvesting coverage rate.

[0009] 2. Low harvesting efficiency: Frequent platform movement or adjustment of robotic arm posture is required to cover fruits in different locations, resulting in an average harvesting time of 8-12 seconds per fruit and increased energy consumption (the proportion of energy consumption for platform turning increases).

[0010] (ii) Multi-arm robotic arm harvesting robot.

[0011] Technical features: It adopts a dual-arm or multi-arm collaborative structure (such as the dual-arm harvesting robot developed by Northwest A&F University) to expand the harvesting range through multi-arm parallel operation.

[0012] Its limitations include:

[0013] 1. Rigid constraints of the workspace: The two arms can only rise and fall synchronously (with a fixed vertical height difference), and cannot independently adjust their height according to the layered distribution of the fruit (e.g., the upper layer of fruit is concentrated at 3-4m, and the lower layer at 1.5-2m). For example, when the right arm is picking the upper layer of fruit, the left arm is limited to a specific height due to synchronous rise and fall, and cannot pick the lower layer of fruit at the same time. The efficiency of the two-arm collaboration is only 20%-30% higher than that of the single arm.

[0014] 2. High control complexity: Multi-arm movement is prone to interference, requiring complex obstacle avoidance algorithms, resulting in system response delay (picking a single fruit takes more than 7.5 seconds).

[0015] (III) Visual system calibration technology.

[0016] Technical features: A static calibration method (such as hand-eye calibration based on the ICP algorithm) is used to establish a mapping relationship between the camera coordinate system and the robot arm base coordinate system when the robot arm is stationary.

[0017] However, it has the drawback of dynamic environment calibration failure: the movement of the robotic arm or the change in the pose of the moving platform (such as the tilting of the platform due to terrain undulation) will destroy the original calibration mapping relationship, resulting in an increase in visual positioning error (measured up to 5-10mm, far exceeding the allowable error of 2mm for fruit grasping), causing fruit grasping failure (probability of about 15%) or branch collision (probability of about 8%).

[0018] II. Analysis of the causes of defects in existing technologies.

[0019] (a) Causes of low harvesting efficiency

[0020] The path planning algorithm adopts the shortest path model used in industrial scenarios (such as Dijkstra's algorithm) and is not optimized for the unstructured environment of orchards. For example, the traditional RRT* algorithm uses uniform sampling throughout the space, which results in redundant sampling in sparse areas of branches and insufficient sampling in dense areas, leading to low planning efficiency (single path planning time > 200ms).

[0021] Single-arm systems need to expand their coverage area by moving the platform, while dual-arm systems suffer from the "one arm idle" problem due to synchronous lifting and lowering (e.g., when the right arm is picking fruit from the upper layer, the left arm has no fruit to pick at the same height). Path planning does not incorporate agronomic characteristics of fruit trees (such as branch density and fruit distribution models), resulting in an idle stroke rate of up to 30% for the robotic arm.

[0022] (II) Causes of dynamic visual calibration failure.

[0023] Visual calibration technology originated from static industrial inspection scenarios (such as parts positioning on assembly lines), and its algorithms (such as the PnP algorithm) did not consider dynamic displacement compensation. When it was adapted to mobile harvesting robots, the developers did not establish a real-time correlation model of "robotic arm movement - coordinate system change - calibration parameter correction", resulting in uncontrollable calibration accuracy in dynamic scenarios.

[0024] Static calibration assumes that the robot's base coordinate system is relatively fixed with the world coordinate system. However, in actual harvesting, the robot's lifting (vertical displacement of 1-2m) or platform tilting (tilt angle of 5°-10°) will cause the origin of the base coordinate system to shift or rotate, thus destroying the original calibration matrix.

[0025] III. Necessity of Improvement

[0026] The core contradiction of existing technologies lies in the mismatch between the rigid constraints of the workspace and the complexity of the orchard environment, and the incompatibility between static calibration methods and the dynamic motion of the robotic arm. This results in low harvesting coverage (<60%), low efficiency (time per fruit >7.5 seconds), and high fruit loss rate (8%). The essence of these problems is that existing technologies have not broken through the path dependence of structural design, have not integrated agronomic features for intelligent planning, and have not established a vision-robotic arm coordinate association model in dynamic environments.

[0027] Therefore, there is an urgent need to develop an apple picking system that can achieve independent lifting and lowering of both arms to expand the workspace, integrate agronomic features to optimize collaborative tasks, and dynamically correct visual calibration parameters, so as to overcome the limitations of existing technologies and promote the practical application of apple picking robots. Summary of the Invention

[0028] This invention provides an apple picking system that enables dynamic adjustment of the workspace, thereby overcoming the shortcomings of existing technologies that lack adaptability to complex orchard environments due to fixed workspaces.

[0029] To achieve the above objectives, the dual six-degree-of-freedom variable workspace robotic arm of the present invention includes a mobile platform, two six-degree-of-freedom robotic arms, a scissor-type lifting unit that drives each robotic arm to lift independently, an end effector, and a control system that integrates a fruit recognition and branch extraction module, a dynamic visual calibration module, and a dual-arm collaborative control module.

[0030] The scissor lifting unit is connected to the servo motor drive screw through an independent X-shaped cross hinge mechanism to achieve single-arm vertical lifting;

[0031] The fruit recognition and branch extraction module is used to obtain the three-dimensional coordinates of the target fruit and the topological structure of the branch;

[0032] The dynamic vision calibration module uses a dual-thread parallel processing of a laser rangefinder and a vision sensor to correct the coordinate mapping relationship in the movement of the robotic arm in real time.

[0033] The dual-arm collaborative control module uses the target fruit's three-dimensional coordinates and branch topology output by the fruit recognition and branch extraction module to achieve dynamic task allocation and safe path planning for both arms.

[0034] The dual-arm collaborative control module includes a dynamic task allocation unit and an agronomic feature fusion path planning unit; the dynamic task allocation unit allocates fruit picking tasks based on a spatiotemporal benefit cost function, which is:

[0035]

[0036] Cost k It is the total cost of the k-th robotic arm picking a certain fruit, which is dimensionless;

[0037] α = 0.6, β = 0.3, γ = 0.1; α is the time cost weighting coefficient, which is dimensionless;

[0038] β is the collision risk weighting coefficient, which is dimensionless;

[0039] γ is the dimensionless coefficient of the high-fit penalty weight.

[0040] armk refers to the current position coordinates of the origin of the base coordinate system of the robotic arm k, in meters;

[0041] "Fruit" refers to the three-dimensional coordinates of the target fruit, that is, the spatial position of the apple to be picked by the robotic arm in the world coordinate system, in meters, and is output by the fruit recognition and branch extraction module.

[0042] v max It is the maximum permissible linear velocity of the robotic arm, measured in meters per second. It is an inherent parameter that existed prior to this invention and is determined by the hardware of the robotic arm, belonging to the physical limitations of the equipment.

[0043] Risk collision It is a collision risk score on the picking path, which is dimensionless and is a design parameter of this invention. It is obtained by weighted integral of the distance between all points on the path and the nearest branch or normalized by the maximum violation value.

[0044] Height penalty It is a highly adaptive penalty term, dimensionless, and is a design parameter of this invention. (Height) penalty =EXP(|Harm-Hfruit|), where Harm is the current height of the end effector of the robotic arm in meters, and is the status feedback data of the robotic arm;

[0045] Hfruit represents the height of the target fruit in meters, which is extracted by the fruit recognition and branch extraction module using the YOLOv5s model and the RealSense D455i vision sensor output three-dimensional coordinate set Pf = (xi, yi, zi) for height coordinate zi.

[0046] The path planning unit sets a safety distance based on the branch diameter: 50 mm when the branch diameter is ≥15 mm and 30 mm when the branch diameter is <15 mm.

[0047] The end effector is a flexible-rigid composite structure, including a silicone elastic contact layer, an integrated force sensor, and a miniature pneumatic cutting blade with a cutting force ≤2N.

[0048] The scissor lift unit includes a bottom frame fixed to the upper surface of the mobile platform and a lift platform. The bottom surface of the lift platform is provided with an upper frame, which is located directly above the bottom frame.

[0049] The bottom frame is connected to two parallel and spaced lower rails, and the top frame is connected to two parallel and spaced upper rails. The upper rails and lower rails correspond one-to-one, and the corresponding upper and lower rails are connected by an X-shaped cross hinge device.

[0050] Each X-type cross hinge device includes two hinge plates, which are hinged together at the middle by a central axis, and the two hinge plates form an X-shaped cross structure with the central axis as the center.

[0051] One bottom end of the hinge plate rotates downward and engages with the lower hinge short shaft. The lower hinge short shaft passes through the lower hinge seat and is hinged to the lower hinge seat. The lower hinge seat is fixedly connected to the lower rail. The lower hinge short shaft, the lower hinge seat, and the lower rail correspond one-to-one. The other bottom end of the hinge plate rotates downward and engages with the moving shaft. A lower moving wheel is installed on the moving shaft and rolls with the lower rail.

[0052] The top edge of one side of the hinge plate rotates upward and engages with the upper hinge short shaft. The upper hinge short shaft passes through the upper hinge seat and is hinged to the upper hinge seat. The upper hinge seat is fixedly connected to the upper rail. The upper hinge short shaft, the upper hinge seat, and the upper rail correspond one-to-one. The top edge of the other side of the hinge plate rotates upward and engages with the upper moving wheel. The upper moving wheel rolls with the upper rail.

[0053] The lower hinge short shaft, lower hinge seat, lower rail and lower moving wheel correspond one-to-one with the upper hinge short shaft, upper hinge seat, upper rail and upper moving wheel;

[0054] The two sets of X-type cross hinge devices share the same moving axis and the same central axis.

[0055] The base frame is connected to a servo motor, and the output shaft of the servo motor is connected to a lead screw via a coupling. A transmission block is fixedly connected to the middle of the moving shaft, and the middle of the lead screw passes through the transmission block and is threaded into the transmission block. The other end of the lead screw is connected to the base frame via a bearing seat. Two sets of X-type cross hinge devices form an X-type cross hinge mechanism.

[0056] The scissor lifting unit includes two sets of independent X-shaped cross hinges, which are hinged by a central pin. The servo motor drives the lead screw to push the bottom corner of the hinge to achieve extension and retraction. The single arm lifting speed is 0.1~0.3m / s, and the stroke is 0-1.2m.

[0057] The scissor lift unit features a height difference constraint and dynamic compensation mechanism for the two arms, specifically:

[0058] The height difference between the two arms of the scissor lift unit is constrained to |H1-H0|≤800mm; when the actual height difference is detected to exceed the threshold, a dynamic compensation mechanism is triggered: the lifting speed of the lagging arm is increased by 1.2 times until the height difference is ≤600mm and then the nominal speed is restored.

[0059] The control system is an industrial control computer integrated into the mobile platform. It realizes joint status acquisition, trajectory planning and actuator control through ROS nodes. It communicates with the robotic arm via cable, and the response delay is ≤20ms.

[0060] This invention also discloses a dual-arm collaborative harvesting control method using the above-mentioned dual six-degree-of-freedom variable workspace robotic arm, comprising the following steps:

[0061] S1. Fruit recognition and branch extraction: The three-dimensional coordinates of the fruit are identified by the YOLOv5s model, and the topology and diameter parameters of the branches are extracted by the PointNet++ network.

[0062] S2. Dynamic task allocation: Calculate the dual-arm harvesting cost based on the spatiotemporal benefit cost function, and use the Hungarian algorithm to output the fruit-robotic arm mapping relationship;

[0063] S3. Agronomic Path Planning: Based on the branch diameter classification, safety distances are set. An obstacle avoidance trajectory is generated using an improved RRT* algorithm. In the improved RRT* algorithm, branches with a diameter ≥15mm are marked as rigid branches and a safety distance of 50mm is set (hard constraint), and path points must be greater than or equal to 50mm. Branches with a diameter <15mm are marked as flexible branches and a safety distance of -30mm is set (soft constraint), allowing path points to penetrate elastically by less than 30mm. That is, when penetrating, the robotic arm is allowed to push the flexible branch to produce elastic deformation within 30mm.

[0064] S4. Dynamic visual calibration: When the robotic arm is raised and lowered, the laser rangefinder measures the displacement Δd for coarse compensation, and simultaneously optimizes the visual pose residual ΔTerror through the Levberg-Marquardt algorithm.

[0065] S5. End-effector control: The flexible-rigid composite actuator adjusts the gripping force based on feedback from the force sensor, and the pneumatic cutter cuts the fruit stem.

[0066] In step S3, the improved RRT* algorithm uses a sampling density three times that of the region >500mm from the fruit within a range of <200mm. The trajectory is smoothed using fifth-order polynomial interpolation, and the joint acceleration is ≤10 rad / s². 2 .

[0067] In step S4, the dynamic visual calibration includes two stages: initial static calibration and dynamic incremental compensation.

[0068] Initial static calibration: achieved by combining PnP algorithm with RANSAC robust estimation. 40 sets of images were acquired in 8 different poses using the ArUco calibration board. The transformation relationship between the visual sensor coordinate system and the world coordinate system was solved, and the initial reprojection error was <0.3 pixels. The coordinate systems of the dual-arm base were aligned by the end effector contacting the shared calibration point to ensure uniform pose of the dual arms.

[0069] Dynamic incremental compensation: Dual-thread parallel processing is adopted: Thread 1 performs coarse compensation by measuring the displacement Δd using a laser rangefinder, with a time of ≤5ms; Thread 2 acquires images using the Realsense D455i vision sensor and performs fine compensation by optimizing the pose residual ΔTerror using the Levenberg-Marquardt algorithm, with a time of ≤20ms.

[0070] The industrial control computer has a segmented pre-stored architecture and a dynamic hierarchical strategy;

[0071] Pre-stored architecture: Based on the initial static calibration results, calibration parameter packages are pre-stored at vertical displacements of 200 / 400 / 600mm. The coordinate mapping relationship is directly called to reduce the initial calculation time of dynamic compensation and to quickly call the calibration results to reduce the amount of real-time calculation.

[0072] Dynamic grading strategy: When the actual displacement exceeds the pre-stored position, incremental compensation is triggered, and the residual is optimized by the Levenberg-Marquardt algorithm to ensure the positioning error is ≤1mm.

[0073] The present invention has the following advantages:

[0074] By independently lifting and dynamically calibrating, the rigid constraints of existing dual-arm synchronous lifting are broken, enabling the two robotic arms to independently adjust their height according to the layered fruit distribution of the fruit trees, thereby increasing the fruit coverage of the canopy, reducing positioning errors, meeting the requirements for harvesting accuracy, and solving the problems of insufficient rigidity and dynamic accuracy in the workspace of traditional systems.

[0075] By allocating fruit picking tasks based on the spatiotemporal benefit cost function, the idle travel of the robotic arm is avoided, the task allocation efficiency is greatly improved, the load balance of the two arms is increased, the picking time of a single fruit is reduced, the picking efficiency is improved, and the probability of unsafe collisions with branches is reduced (excluding safe collisions).

[0076] The scissor-type lifting unit enables independent adaptation of the two arms to the canopy layers (such as picking upper fruits with the high arm and picking inner fruits with the low arm), thereby improving the picking coverage.

[0077] The scissor lift unit is configured with a maximum lifting speed of 0.3m / s. At the maximum lifting speed, it can complete a 600mm height adjustment within 2 seconds. Even at the lower lifting speed, it can achieve relatively fast height adjustment to meet the needs of rapid adaptation of layered fruits, while ensuring platform stability to maintain visual calibration accuracy.

[0078] The following technical advantages are achieved through the structural design of the scissor lift unit:

[0079] 1. High structural stability and motion precision ensure accurate harvesting positioning.

[0080] X-type cross hinge device: Each device is hinged in an X shape by the central axis in the middle of two hinge plates. Combined with the upper and lower rails, moving wheels and hinge seats, it can achieve smooth guidance during the lifting process and avoid the robot arm from shaking.

[0081] Servo motor driven lead screw transmission: The lead screw is threadedly engaged with the transmission block on the moving shaft. By precisely controlling the rotation of the lead screw, the linear displacement of the lifting platform is achieved, with a positioning accuracy of ±0.5mm, ensuring the precise positioning of the fruit by the end effector of the robotic arm (positioning error ≤1mm after dynamic compensation).

[0082] Materials and load-bearing design: The hinges are made of No. 45 steel, and each scissor unit can bear a load of ≥50kg, ensuring structural rigidity and adapting to stable operation in complex orchard terrain (such as hilly slopes and obstacles between rows).

[0083] 2. Modular structural design facilitates maintenance and expansion. The scissor lift unit's base frame, lifting platform, upper and lower rails, and X-type cross hinge device are modularly assembled. Each component (such as the lower hinge shaft, moving wheels, and lead screw) is independently detachable, facilitating later maintenance and replacement. Furthermore, the two sets of X-type cross hinge devices share the moving shaft and central shaft, simplifying the structure and reducing manufacturing costs. It can be expanded into a multi-arm collaborative system according to actual needs.

[0084] The height difference constraint and dynamic compensation mechanism of the scissor lift unit's two arms bring the following advantages:

[0085] 1. Prevent interference between the two arms and ensure the safety of the mechanical structure. By setting a height difference constraint of |H1-H0|≤800mm between the two arms, the maximum relative displacement of the two robotic arms in the vertical direction is clearly limited, avoiding physical collisions or overlapping motion trajectories between the robotic arms due to excessive raising / lowering of a single arm (such as the intersection of the working space of the two arms in a dense tree canopy area). This ensures the motion safety of the two arms when they are raised and lowered independently from the structural level, and solves the problem of "high risk of cooperative interference" in traditional multi-arm systems.

[0086] 2. Dynamic compensation improves the efficiency of dual-arm coordination and reduces idle travel time. When the actual height difference exceeds the 800mm threshold, the lagging arm quickly catches up by increasing its speed by 1.2 times until the height difference returns to within 600mm and the nominal speed is restored. This mechanism avoids the inefficient coordination mode where one arm waits for the other to adjust (e.g., when the right arm is picking upper-level fruit, the left arm does not need to wait for the right arm to descend for a long time to adapt to the height of the lower-level fruit by increasing its speed), significantly reducing the idle travel time caused by height adaptation between the two arms.

[0087] 3. Adapting to the layered fruit distribution of fruit trees and improving harvesting coverage. The combination of height difference constraint (≤800mm) and dynamic compensation allows the two robotic arms to independently raise and lower to adapt to the layered fruit distribution of fruit trees, and also to quickly and collaboratively cover scattered fruits in the same vertical area (such as fruits at adjacent heights in the middle of the canopy). This avoids blind spots in local harvesting caused by excessive height difference between the two arms, thereby improving the canopy fruit coverage.

[0088] 4. Balancing energy consumption and response speed to optimize system operating economy. The dynamic compensation mechanism only triggers the lag arm speed increase when the height difference exceeds the limit, maintaining the nominal speed (0.1~0.3m / s) under normal conditions to avoid energy waste caused by continuous high-speed lifting; the 1.2 times speed increase ratio takes into account both rapid response and the load of the robotic arm drive system (5mm lead of the scissor lifting unit lead screw, positioning accuracy ±0.5mm), preventing a decrease in positioning accuracy or mechanical wear caused by excessive speed increase, and achieving a balance of "high efficiency collaboration - low energy consumption - high precision".

[0089] 5. Enhance adaptability to unstructured terrain and ensure system stability.

[0090] The mobile platform uses differential drive to adapt to unstructured terrain such as hills and gullies. The tilting of fruit trees or undulating terrain may cause fluctuations in the initial height difference between the two arms. The height difference constraint and dynamic compensation mechanism ensures that the two arms can maintain a reasonable operating height range under terrain disturbances through real-time monitoring and adjustment, avoiding harvesting interruptions often caused by height differences due to terrain, and improving the robustness of the system in complex orchard environments.

[0091] The scissor lift unit achieves the technical effects of "independent lifting, precise positioning, rapid response, and stable reliability" through structural innovation, laying the core hardware foundation for improving the coverage, efficiency, and environmental adaptability of apple harvesting systems.

[0092] The sampling density is denser near the edge and sparser further away. The fifth-order polynomial interpolation smooths the trajectory, balancing the efficiency and accuracy of path planning. The path length is shortened compared to the traditional RRT*, reducing the average curvature of the path and correspondingly reducing the energy consumption of the robotic arm.

[0093] The segmented pre-store architecture and dynamic hierarchical strategy bring the following technical advantages:

[0094] 1. Reduce real-time computation and improve calibration response speed. The segmented pre-storage architecture pre-stores static visual calibration packages at key vertical displacement positions (e.g., 200 / 400 / 600mm). During actual harvesting, the pre-calculated coordinate mapping relationship can be directly called, avoiding the need for the robotic arm to re-execute the complete calibration process (e.g., the acquisition of 40 sets of images and PnP algorithm solution for initial static calibration) for each lifting and lowering. This significantly reduces the real-time computation time of the dynamic visual calibration module, providing hardware resource support for the real-time requirement of "calibration cycle ≤ 25ms".

[0095] 2. Dynamically adapts to complex displacement scenarios, ensuring calibration accuracy throughout the entire stroke. The dynamic hierarchical strategy adapts to the robotic arm's 0-1.2m full stroke through a two-level mechanism of "pre-stored recall - incremental compensation":

[0096] Pre-stored positions (such as 200 / 400 / 600mm) can be directly called from the calibration package, and the positioning accuracy relies on the <0.3 pixel reprojection error of the initial static calibration;

[0097] The non-pre-stored position activates the incremental fine-tuning mode, which uses laser coarse compensation (BenewakeTF03 rangefinder, time ≤5ms) and visual fine-tuning (Levenberg-Marquardt algorithm to optimize residuals, time ≤20ms) to ensure that the positioning error after dynamic compensation is ≤1mm, thus solving the problem of decreased accuracy of traditional static calibration at non-preset positions.

[0098] 3. Enhance the orchard's environmental adaptability and cover the fruit distribution in layers.

[0099] To address the uneven distribution of fruit in stratified fruit trees (such as differences in fruit density at different heights of the canopy), the segmented pre-storage architecture can optimize the pre-storage location based on orchard measurement data (such as 500 sets of orchard data, claim 2), prioritizing coverage of areas with concentrated fruit. The dynamic grading strategy, on the other hand, quickly adapts to the height requirements of scattered fruit distribution through incremental compensation, avoiding calibration drift caused by frequent lifting and lowering of the robotic arm, and improving the "canopy fruit coverage rate".

[0100] 4. Balance system energy consumption and computing resources to optimize operating economy.

[0101] The segmented pre-stored architecture reduces redundant calculations by pre-stored calibration packages, thereby lowering the CPU utilization of the industrial control computer (the core hardware of the control system). The dynamic hierarchical strategy triggers incremental compensation only at non-pre-stored locations, avoiding energy waste caused by continuous high-precision calculations throughout the entire process. This achieves coordinated optimization of "efficient calibration - low resource consumption - low energy consumption," meeting the economic requirements for the practical application of agricultural robots.

[0102] 5. Ensure calibration stability during high-speed lifting and lowering of the robotic arm.

[0103] The robotic arm has a maximum lifting speed of 0.3 m / s. Traditional dynamic calibration is prone to coordinate mapping lag due to computational delays. The segmented pre-stored architecture shortens the initial response time through "pre-calculation + fast recall", and the dynamic hierarchical strategy ensures real-time compensation through dual-thread parallel processing (laser + vision). The combination of the two enables the calibration system to maintain a positioning error of ≤1 mm even in high-speed lifting scenarios, providing stable coordinate guidance for the end effector (robotic gripper) to grasp accurately.

[0104] The dual-thread parallel processing solves large-scale displacement through laser coarse compensation and visual fine correction eliminates calibration drift caused by environmental interference (such as changes in lighting and shading by branches), reducing the total time of dynamic calibration compared to single-thread calibration and meeting the real-time requirements of the robotic arm at a maximum lifting speed of 0.3m / s.

[0105] Initial static calibration establishes a high-precision benchmark and reduces cumulative errors in dynamic compensation: By combining the PnP algorithm with RANSAC robust estimation, 40 sets of images were acquired using the ArUco calibration board in 8 different poses to solve the initial transformation relationship between the vision sensor and the world coordinate system. The initial reprojection error was <0.3 pixels, providing a high-precision benchmark for dynamic incremental compensation. At the same time, the coordinate system alignment of the two arms bases was achieved by contacting the shared calibration point of the end effector, avoiding the cooperative error caused by the coordinate system deviation of the two arms, and reducing the initial error accumulation of dynamic calibration from the source.

[0106] The layered compensation mechanism balances speed and accuracy, adapting to complex orchard environments: The dynamic incremental compensation adopts a layered strategy of "laser coarse compensation + visual fine correction": The laser rangefinder (BenewakeTF03) quickly measures the displacement Δd (time ≤ 5ms), solving the problem of large-scale rigid displacement caused by the lifting and lowering of the robotic arm; The vision sensor (RealsenseD455i) optimizes the pose residual ΔTerror through the Levenberg-Marquardt algorithm (time ≤ 20ms), accurately eliminating calibration drift caused by environmental interference such as changes in lighting and shading by branches, achieving the dual advantages of "fast response + accurate correction".

[0107] Strict real-time and accuracy indicators ensure harvesting reliability: the total cycle of dynamic visual calibration is ≤25ms, and the ROS multi-threaded callback mechanism achieves efficient thread scheduling, ensuring the real-time response of the robotic arm at a maximum lifting speed of 0.3m / s; the positioning error after dynamic compensation is ≤1mm, which meets the core indicator of "reducing positioning error (≤1mm) to meet the harvesting accuracy requirements" in claim 1, and provides coordinate mapping guarantee for the end effector (robotic claw) to accurately grasp the target fruit. Attached Figure Description

[0108] Figure 1 This is a schematic diagram of the structure of the dual six-degree-of-freedom variable workspace robotic arm apple picking system of the present invention.

[0109] Figure 2 yes Figure 1 AA sectional view.

[0110] Figure 3 yes Figure 1 CC section view.

[0111] Figure 4 yes Figure 1 DD sectional view.

[0112] Figure 5 This is a three-dimensional structural diagram of the present invention.

[0113] Figure 6 yes Figure 3 Enlarged view of point A in the middle.

[0114] Figure 7 yes Figure 4 Enlarged view of point B in the middle.

[0115] Terminology Explanation:

[0116] 6-DOF robotic arm: A robotic arm with 6 rotary joints that can achieve three-dimensional translation and rotation in space.

[0117] Variable workspace: The height of the dual robotic arms can be independently adjusted via a scissor lift unit, allowing the reach of the end effector to dynamically adapt to the distribution of the fruit.

[0118] Dynamic visual calibration: A calibration method that uses a laser rangefinder and a vision sensor to process data in a dual-thread manner during the movement of the robotic arm and corrects the coordinate mapping relationship in real time. Detailed Implementation

[0119] like Figures 1 to 7 As shown, this invention provides a dual-arm apple picking system with a variable workspace, including a mobile platform 1, two 6-DOF robotic arms 2, a scissor-type lifting unit that drives each robotic arm 2 to lift independently (there are two scissor-type lifting units, each corresponding to one robotic arm 2), an end effector 3, i.e., a robotic gripper, installed on the robotic arms 2, and a control system integrating a fruit recognition and branch extraction module, a dynamic visual calibration module, and a dual-arm collaborative control module; the four corners of the mobile platform 1 are respectively connected to walking wheels 4 downwards, and the mobile platform 1 has a drive device (such as a walking motor). The drive device drives the walking wheels 4 through a transmission shaft to achieve movement. The walking motor drives the walking wheels 4 through a transmission shaft to achieve movement, which is a conventional technology. The drive device (walking motor) is not shown in the figure.

[0120] The scissor lift unit includes a bottom frame 7 fixed to the upper surface of the mobile platform 1 and a lifting platform 5. The bottom surface of the lifting platform 5 is provided with an upper frame 6, which is located directly above the bottom frame 7.

[0121] The bottom frame 7 is connected to two parallel and spaced lower rails 8, and the upper frame 6 is connected to two parallel and spaced upper rails 9. The upper rails 9 and the lower rails 8 correspond one-to-one, and an X-shaped cross hinge device is connected between the corresponding upper rails 9 and lower rails 8.

[0122] Each X-type cross hinge device includes two hinge plates 10, the middle of the two hinge plates 10 are hinged together by a central shaft 11, and the two hinge plates 10 form an X-shaped cross structure with the central shaft 11 as the center.

[0123] One bottom end of the hinge plate 10 rotates downward and engages with the lower hinge short shaft 12. The lower hinge short shaft 12 passes through the lower hinge seat 13 and is hinged to the lower hinge seat 13. The lower hinge seat 13 is fixedly connected to the lower rail 8. The lower hinge short shaft 12, the lower hinge seat 13 and the lower rail 8 correspond one-to-one. The other bottom end of the hinge plate 10 rotates downward and engages with the moving shaft 14. A lower moving wheel 15 is installed on the moving shaft 14 and rolls with the lower rail 8.

[0124] The top edge of one side of the hinge plate 10 rotates upward and engages with the upper hinge short shaft 16. The upper hinge short shaft 16 passes through the upper hinge seat 17 and is hinged to the upper hinge seat 17. The upper hinge seat 17 is fixedly connected to the upper rail 9. The upper hinge short shaft 16, the upper hinge seat 17, and the upper rail 9 correspond one-to-one. The top edge of the other side of the hinge plate 10 rotates upward and engages with the upper moving wheel 18. The upper moving wheel 18 rolls with the upper rail 9.

[0125] The lower hinge short shaft 12, lower hinge seat 13, lower rail 8 and lower moving wheel 15 correspond one-to-one with the upper hinge short shaft 16, upper hinge seat 17, upper rail 9 and upper moving wheel 18;

[0126] The two sets of X-type cross hinge devices share the same moving axis 14 and the same central axis 11.

[0127] The bottom frame 7 is connected to a servo motor 19. The output shaft of the servo motor 19 is connected to a lead screw 21 through a coupling 20. A transmission block 22 is fixedly connected to the middle of the moving shaft 14. The middle of the lead screw 21 passes through the transmission block 22 and is threadedly engaged with the transmission block 22. The other end of the lead screw 21 is connected to the bottom frame 7 through a bearing seat 23.

[0128] Two sets of X-type cross hinge devices form an X-type cross hinge mechanism; the scissor lifting unit is connected to the lead screw 21 driven by the servo motor 19 through an independent X-type cross hinge mechanism to achieve vertical lifting of a single arm;

[0129] The fruit recognition and branch extraction module is used to obtain the three-dimensional coordinates of the target fruit and the topological structure of the branch;

[0130] The dynamic visual calibration module uses a dual-thread parallel processing of a laser rangefinder and a visual sensor to correct the coordinate mapping relationship of the robotic arm 2 in real time; the laser rangefinder is a conventional technology and is not shown in the figure.

[0131] The dual-arm collaborative control module uses the target fruit's three-dimensional coordinates and branch topology output by the fruit recognition and branch extraction module to achieve dynamic task allocation and safe path planning for both arms.

[0132] The mobile platform 1 employs differential drive to adapt to unstructured terrain, including hills and gullies; the scissor lift unit has a travel range of 0-1.2m and a positioning accuracy of ±0.5mm; the dynamic vision calibration module includes a Benewake TF03 laser rangefinder and a Realsense D455i vision sensor 24, with a calibration cycle of ≤25ms. The vision sensor 24 is connected to the mobile platform 1 via a support rod 25.

[0133] By independently lifting and dynamically calibrating, the rigid constraints of existing dual-arm synchronous lifting are broken, enabling the dual robotic arms 2 to independently adjust their height according to the layered fruit distribution of the fruit trees, thereby increasing the fruit coverage of the canopy, reducing positioning errors (≤1mm), meeting the requirements for harvesting accuracy, and solving the problems of insufficient rigidity and dynamic accuracy in the working space of traditional systems.

[0134] The fruit recognition and branch extraction module employs a lightweight deep learning model based on YOLOv5s to output the 3D coordinates of the fruit and the topological structure of the branch in real time. The YOLOv5s model has an input resolution of 640×640, an inference time of ≤8ms (based on NVIDIA Jetson AGXXavier), a fruit recognition confidence threshold of ≥0.85, and uses a morphological skeletonization algorithm for branch extraction. This module enables real-time perception of the fruit and branches during harvesting, providing precise positioning guidance for the end effector 3 and reducing invalid grasping actions by 30%. The model was trained on 20,000 real-world orchard images through transfer learning, achieving a recognition accuracy of ≥96% and supporting mainstream apple varieties such as Fuji and Gala.

[0135] The dual-arm collaborative control module includes a dynamic task allocation unit and an agronomic feature fusion path planning unit; the dynamic task allocation unit allocates fruit picking tasks based on a spatiotemporal benefit cost function, which is:

[0136]

[0137] Cost k Cost represents the comprehensive cost of the k-th robotic arm 2 picking a certain fruit; it is dimensionless (a relative score). Design principle: It comprehensively evaluates the time cost, collision risk cost, and high mismatch penalty cost required to pick a certain fruit, using the Hungarian algorithm for optimal task allocation. The smaller the value, the more "cost-effective" it is for robotic arm 2 to pick that fruit. k There are no existing industry standards; this is an evaluation indicator specifically constructed by this invention to achieve "dynamic task allocation".

[0138] α = 0.6, β = 0.3, γ = 0.1; α is the time cost weighting coefficient, dimensionless, and its design principle is derived through optimization using a genetic algorithm on 500 sets of orchard measured data. The fitness function is a weighted average of harvesting efficiency and loss rate. α = 0.6 indicates that the time factor dominates the overall cost. The normalization constraint for α, β, and γ is: α + β + γ = 1;

[0139] β is the collision risk weight coefficient, which is dimensionless. The design principle of β is the same as that of α, and it is obtained through optimization using a genetic algorithm. β = 0.3. It reflects the system's emphasis on avoiding collisions (protecting fruits and branches).

[0140] γ is a dimensionless weighting coefficient for high-fit penalty; it is derived through genetic algorithm optimization and is specifically designed for the "layered fruit picking" scenario, encouraging robotic arm 2 to pick at similar heights, thus reducing lifting energy consumption and time.

[0141] armk refers to the current position coordinates of the origin of the base coordinate system of robotic arm 2k (k = 1, 2, corresponding to the left / right robotic arm 2), which is the core parameter describing the real-time pose of robotic arm 2 in three-dimensional space; armk is obtained by forward kinematics calculation (i.e., solving the position of the end effector 3 in the base coordinate system according to the joint encoder of robotic arm 2) through real-time feedback data of the joint encoder, and the unit is meters (m).

[0142] "Fruit" refers to the three-dimensional coordinates of the target fruit, denoted by "pf". Pf = (xi, yi, zi), which represents the spatial position of the apple to be picked by the robotic arm 2 in the world coordinate system, in meters. This position is output by the fruit recognition and branch extraction module of the perception layer. Specifically, color images and depth information of the orchard scene are acquired through the RealSense D455i vision sensor 24 and input into the lightweight YOLOv5s model for fruit detection, outputting candidate fruit boxes with confidence scores. The pixel coordinates are converted into three-dimensional world coordinates by combining the depth information, ultimately obtaining the set of three-dimensional coordinates of the target fruit. The model is trained based on 500 sets of orchard measurement data (covering varieties such as Fuji and Gala), with a confidence threshold set to ≥0.85 to ensure the accuracy of coordinate extraction. After correction by the dynamic visual calibration module, the fruit coordinate positioning error is ≤1mm.

[0143] v max This is the maximum permissible linear velocity of robotic arm 2, measured in meters per second (m / s). It is an inherent parameter existing prior to this invention, determined by the hardware of robotic arm 2, and is a physical limitation of the device; v max In this invention, it is used to normalize the time cost term, making it comparable.

[0144] Risk collision It is a collision risk score on the harvesting path, dimensionless (0-1 interval or normalized score), and is a design parameter of this invention. It is obtained by weighted integral of the distance between all points on the path and the nearest branch or normalized by the maximum violation value. There is no industry standard. It is an assessment quantity designed by this invention to integrate "agronomic obstacle avoidance". collision The design principle is as follows: based on the branch topology model output by the perception layer, combined with an improved RRT* algorithm, the degree of safety distance violation between path points and branches is calculated during the path planning stage. For example, if a path point intrudes into the 50mm safety zone of a rigid branch, then the Risk... collision Increase.

[0145] Height penaltyIt is a highly adaptive penalty term, dimensionless, and is a design parameter of this invention. (Height) penalty =EXP(|Harm-Hfruit|), where Harm is the current height (z coordinate) of the end effector 3 of the robotic arm 2, in meters, calculated from the joint encoder reading and positive kinematics, and is the state feedback data of the robotic arm 2;

[0146] Hfruit represents the height (z-coordinate) of the target fruit, in meters. It is extracted by the fruit recognition and branch extraction module using the YOLOv5s model and the RealSense D455i vision sensor 24 from the three-dimensional coordinate set Pf = (xi, yi, zi).

[0147] The path planning unit sets a safety distance based on the branch diameter: 50 mm when the branch diameter is ≥15 mm and 30 mm when the branch diameter is <15 mm.

[0148] The cost function weights α = 0.6, β = 0.3, and γ = 0.1 were optimized using a genetic algorithm (population 50, 100 generations) and trained based on 500 sets of orchard test data. The path planning adopted an improved RRT* algorithm, with the sampling density in the near-fruit area being 3 times that in the far-fruit area.

[0149] By allocating fruit picking tasks based on the spatiotemporal benefit cost function, the two idle strokes of the robotic arm are avoided, the task allocation efficiency is greatly improved, the load balance of the two arms is improved, the picking time of a single fruit is reduced, the picking efficiency is improved, and the probability of unsafe collisions with branches is reduced (excluding safe collisions).

[0150] The end effector 3 is a flexible-rigid composite structure, including a silicone elastic contact layer with a Shore hardness of 40A, an integrated force sensor (contact stress ≤15kPa), and a miniature pneumatic cutting blade (blade thickness 0.3mm) with a cutting force ≤2N.

[0151] The actuator weighs ≤350g, the cutting response time is ≤20ms, and the force sensor sampling frequency is 500Hz.

[0152] Fruit breakage rate <0.5%, fruit stem cutting time ≤0.3 seconds, significantly improving harvesting integrity compared to traditional actuators.

[0153] The scissor lifting unit includes two sets of independent X-shaped cross hinges, which are hinged by a central pin. The servo motor 19 drives the lead screw 21 to push the bottom corner of the hinge to achieve extension and retraction. The single arm lifting speed is 0.1~0.3m / s, and the stroke is 0-1.2m.

[0154] The scissor lift unit features a height difference constraint and dynamic compensation mechanism for the two arms, specifically:

[0155] The height difference between the two arms of the scissor lift unit is constrained to |H1-H0|≤800mm; when the actual height difference is detected to exceed the threshold, a dynamic compensation mechanism is triggered: the lifting speed of the lagging arm is increased by 1.2 times until the height difference is ≤600mm and then the nominal speed is restored.

[0156] The hinge is made of 45# steel, the single scissor unit has a load capacity of ≥50kg, the lead screw has a lead of 5mm and a positioning accuracy of ±0.5mm.

[0157] The scissor-type lifting unit enables independent adaptation of the two arms to the canopy layers (such as picking upper fruits with the high arm and picking inner fruits with the low arm), thereby improving the picking coverage.

[0158] The scissor lift unit is configured with a maximum lifting speed of 0.3m / s. At the maximum lifting speed, it can complete a 600mm height adjustment within 2 seconds. Even at the lower lifting speed, it can achieve relatively fast height adjustment to meet the needs of rapid adaptation of layered fruits, while ensuring platform stability to maintain visual calibration accuracy.

[0159] The height difference constraint and dynamic compensation mechanism of the scissor lift unit's two arms bring the following advantages:

[0160] 1. Prevent interference between the two arms and ensure the safety of the mechanical structure. By setting a height difference constraint of |H1-H0|≤800mm between the two arms, the maximum relative displacement of the two robotic arms 2 in the vertical direction is clearly limited. This avoids physical collisions or overlapping motion trajectories between the two robotic arms 2 due to excessive raising / lowering of a single arm (such as the intersection of the working space of the two arms in a dense tree canopy area). From a structural perspective, this ensures the motion safety of the two arms when they are raised and lowered independently, and solves the problem of "high risk of cooperative interference" in traditional multi-arm systems.

[0161] 2. Dynamic compensation improves the efficiency of dual-arm coordination and reduces idle travel time. When the actual height difference exceeds the 800mm threshold, the lagging arm quickly catches up by increasing its speed by 1.2 times until the height difference returns to within 600mm and the nominal speed is restored. This mechanism avoids the inefficient coordination mode where one arm waits for the other to adjust (e.g., when the right arm is picking upper-level fruit, the left arm does not need to wait for the right arm to descend for a long time to adapt to the height of the lower-level fruit by increasing its speed), significantly reducing the idle travel time caused by height adaptation between the two arms.

[0162] 3. Adapt to the layered fruit distribution of fruit trees and improve harvesting coverage. The combination of height difference constraint (≤800mm) and dynamic compensation enables the dual robotic arms 2 to independently raise and lower to adapt to the layered fruit distribution of fruit trees, and also to quickly and collaboratively cover scattered fruits in the same vertical area (such as fruits at adjacent heights in the middle layer of the canopy), avoiding blind spots in local harvesting caused by excessive height difference between the two arms, and improving the canopy fruit coverage.

[0163] 4. Balancing energy consumption and response speed to optimize system operating economy. The dynamic compensation mechanism only triggers the lag arm speed increase when the height difference exceeds the limit, maintaining the nominal speed (0.1~0.3m / s) under normal conditions to avoid energy waste caused by continuous high-speed lifting; the 1.2 times speed increase ratio takes into account both rapid response and the load of the robotic arm 2 drive system (scissor lifting unit lead screw 21 5mm lead, positioning accuracy ±0.5mm), preventing the decrease in positioning accuracy or mechanical wear caused by excessive speed increase, and achieving a balance of "high efficiency collaboration - low energy consumption - high precision".

[0164] 5. Enhance adaptability to unstructured terrain and ensure system stability.

[0165] Mobile platform 1 employs differential drive to adapt to unstructured terrain such as hills and gullies. The tilting of fruit trees or undulating terrain may cause fluctuations in the initial height difference between the two arms. The height difference constraint and dynamic compensation mechanism ensures that the two arms can maintain a reasonable operating height range under terrain disturbances through real-time monitoring and adjustment, avoiding harvesting interruptions often caused by height differences due to terrain, and improving the robustness of the system in complex orchard environments.

[0166] 5. The system according to claim 1, characterized in that: the control system is an industrial control computer integrated into the mobile platform 1, which realizes joint status acquisition, trajectory planning and actuator control through ROS nodes, and communicates with the robotic arm 2 via cable, with a response delay ≤20ms.

[0167] The industrial control computer is equipped with an Intel i7-10700 processor, 8GB of memory, and supports dual-thread parallel processing (laser + vision calibration). Data interaction between modules is achieved through a topic publish / subscribe model, such as "fruit coordinates topic" and "trajectory command topic," ensuring real-time control.

[0168] The industrial computer of the control system is fixed on the mobile platform 1. The ROS node is a communication unit in the ROS system of the industrial computer that realizes data interaction between modules. The joint controller is configured through the ROS function package (ROS node is a component of the ROS function package) to realize the linkage control of software and hardware.

[0169] The dual-arm collaborative control module achieves data interaction through ROS nodes: the hardware interface node collects data from the joint encoder and force sensor, transmits it to the joint status management node to store the real-time status (position, speed, torque), and broadcasts it to the "left arm joint status topic" and "right arm joint status topic" through the joint status publishing node; after the trajectory controller subscribes to the topic, it generates the motion trajectory and issues position commands at a frequency of 100Hz, with a repeatability accuracy of ±0.01°.

[0170] The present invention also discloses a dual-arm cooperative harvesting control method using the system, comprising the following steps:

[0171] S1. Fruit recognition and branch extraction: The three-dimensional coordinates of the fruit are identified by the YOLOv5s model, and the topology and diameter parameters of the branches are extracted by the PointNet++ network.

[0172] S2. Dynamic task allocation: Calculate the dual-arm harvesting cost based on the spatiotemporal benefit cost function, and use the Hungarian algorithm to output the fruit-robotic arm 2 mapping relationship;

[0173] S3. Agronomic Path Planning: Based on the branch diameter classification, a safety distance is set. An obstacle avoidance trajectory is generated by an improved RRT* algorithm. In the improved RRT* algorithm, branches with a diameter ≥ 15 mm are marked as rigid branches and a safety distance of 50 mm is set, which is a hard constraint. The path point must be greater than or equal to 50 mm. Branches with a diameter < 15 mm are marked as flexible branches and a safety distance of -30 mm is set, which is a soft constraint. The path point is allowed to have elastic penetration of less than 30 mm. That is, when penetrating, the robotic arm 2 is allowed to push the flexible branch to produce an elastic deformation of less than 30 mm.

[0174] S4. Dynamic visual calibration: When the robotic arm 2 is raised and lowered, the laser rangefinder measures the displacement Δd for coarse compensation, and simultaneously optimizes the visual pose residual ΔTerror through the Levberg-Marquardt algorithm.

[0175] S5. End-effector control: The flexible-rigid composite actuator adjusts the gripping force based on feedback from the force sensor, and the pneumatic cutter cuts the fruit stem.

[0176] The improved RRT* algorithm in this invention should be understood as follows:

[0177] I. Objectives and Application Scenarios

[0178] In the apple picking robot, a collision-free, smooth motion trajectory that conforms to agronomic constraints is planned for the robotic arm 2 from its current position to the target fruit.

[0179] Input: Robotic arm 2 starting pose, target fruit pose, environmental obstacle model (mainly fruit tree branch point cloud + semantic classification).

[0180] Output: A smooth sequence of trajectories in joint space or Cartesian space, satisfying:

[0181] Avoid collisions (especially to prevent damage to fruit and branches);

[0182] Consider the differences in branch rigidity / flexibility (agronomic constraints);

[0183] Trajectory continuity, smooth velocity / acceleration (fifth-order polynomial interpolation);

[0184] Real-time performance (150ms update cycle).

[0185] II. A brief review of the core steps of the standard RRT* algorithm.

[0186] Initialization: Build a tree T = {start} from the starting point.

[0187] Sampling: Randomly sample points x_rand in the configuration space.

[0188] Nearest neighbor search: Find the node x_nearest that is closest to x_rand in tree T.

[0189] Extension: Extend one step from x_nearest towards x_rand to obtain a new node x_new (fixed or adaptive step size).

[0190] Collision detection: If there is no collision on the path x_nearest→x_new, then add it to the tree.

[0191] Rewire:

[0192] Find neighboring nodes within a radius r around x_new;

[0193] Try using x_new as the parent node to see if it can reduce the path cost of the neighbors → if so, reconnect;

[0194] Try using a neighbor as the parent node of x_new to see if it can reduce the path cost of x_new → if so, reconnect.

[0195] Iteration: Repeat steps 2 through 6 until the target is found or the maximum number of iterations is reached.

[0196] Path extraction: Tracing back from the target point to the starting point to obtain the initial path.

[0197] The standard RRT* guarantees asymptotic optimality, but does not take into account "semantic barriers" or "differentiated safety distances".

[0198] III. Specific implementation of the "Improved RRT* Algorithm" of this invention.

[0199] Improvement 1: Obstacle semantic classification and safe distance model.

[0200] Input: Branch point cloud model Pb (from the perception layer, PointNet++ skeletonization algorithm);

[0201] Each branch segment has a "diameter" attribute (unit: mm).

[0202] Processing: Classification thresholds are set as follows: Diameter ≥ 15mm → Rigid branches, safety distance 50mm, hard constraint, path point must be greater than the safety distance; Diameter < 15mm → Flexible branches, safety distance -30mm, soft constraint, path point is allowed elastic penetration of less than 30mm, i.e., during penetration, robotic arm 2 is allowed to push the flexible branch to produce elastic deformation within 30mm. The 15mm threshold is based on 200 branch bending failure experiments, with a yield strength of 35N for a 15mm diameter branch, corresponding to a 50mm deformation safety margin.

[0203] Implementation method:

[0204] In the "collision detection" step of RRT*, it is not just about detecting "whether there is a collision", but also about calculating "the distance from the path segment to the nearest branch";

[0205] Apply the corresponding safety distance threshold based on the branch's "rigid / flexible" label;

[0206] If a hard constraint is violated, a collision is detected, and the path segment is discarded.

[0207] If a soft constraint is violated, the value is not discarded, but a "penalty term" or "risk score" can be introduced into the cost function.

[0208] Improvement 2: Differentiated obstacle avoidance strategies are integrated into RRT* extensions and reconnection.

[0209] Modify the standard RRT* "collision detection" function:

[0210] defis_collision_free(x_from,x_to,branch_model):

[0211] # Generate several detection points (e.g., 10) on the path segment;

[0212] waypoints=interpolate(x_from,x_to,num=10);

[0213] forwpinwaypoints:

[0214] #Query the distance and attributes from wp to the nearest branch. wp is an abbreviation for waypoints, which are path points or trajectory sampling points.

[0215] nearest_branch=find_nearest_branch(wp,branch_model);

[0216] d=distance(wp,nearest_branch);

[0217] diameter=nearest_branch.diameter;

[0218] ifdiameter>=15.0:#rigid branch;

[0219] ifd<50.0:# Violation of hard constraint;

[0220] returnFalse;

[0221] else: # Flexible branches;

[0222] ifd>=-30mm:# Allows elastic penetration;

[0223] returnFalse;

[0224] #“Maximum allowable elastic penetration of 30mm”, i.e., d>=-30mm;

[0225] #The physical distance cannot be negative, meaning "the branch is allowed to be pushed back by an equivalent distance of 30mm with elasticity";

[0226] returnTrue.

[0227] It can be simplified to:

[0228] if diameter < 15.0:

[0229] allowed_penetration=30.0#mm;

[0230] ifd<-allowed_penetration: # Exceeds the allowed penetration depth;

[0231] returnFalse.

[0232] Improvement 3: Path post-processing - fifth-order polynomial interpolation smoothing.

[0233] The RRT* output is a "polyline path", which makes the joint movements uneven and easily causes vibration or impact to the robotic arm 2.

[0234] A quintic polynomial can guarantee the continuity of position, velocity, and acceleration (C). 2 continuous).

[0235] enter:

[0236] The initial path point sequence generated by RRT* is: P0, P1, P2, ..., Pm (Cartesian space or joint space).

[0237] deal with:

[0238] For each segment Pi→Pi+1, use fifth-order polynomial interpolation:

[0239] θ(t)=a0+a1·t+a2·t 2 +a3·t 3 +a4·t 4 +a5·t 5 ;

[0240] Where t∈[0,T], and T is the motion time of that segment.

[0241] Boundary conditions (ensure smoothness):

[0242] In S1, the YOLOv5s confidence threshold is ≥0.85; in S4, the calibration period is ≤25ms; and in S5, the gripping force control accuracy is ±0.5N. The single fruit picking time is 3.2 seconds (compared to 7.5 seconds for traditional dual-arm harvesting), the fruit loss rate is 2.5% (compared to 8% for traditional harvesting), and the dynamic environmental positioning error is ≤1mm.

[0243] In step S3, the improved RRT* algorithm uses a sampling density three times that of the region >500mm from the fruit within a range of <200mm. The trajectory is smoothed using fifth-order polynomial interpolation, and the joint acceleration is ≤10 rad / s². 2 .

[0244] The sampling density is denser near the edge and sparser further away. The fifth-order polynomial interpolation smooths the trajectory, balancing the efficiency and accuracy of path planning. The path length is shortened compared to the traditional RRT*, reducing the average curvature of the path and correspondingly reducing the energy consumption of the robotic arm.

[0245] When using this method, it should be used in conjunction with an orchard branch density distribution map, and the sampling frequency should be ≥100Hz to meet real-time requirements. The agronomic path planning unit uses fifth-order polynomial interpolation to generate a smooth trajectory, with the formula θ(t)=a0+a1t+a2t. 2 +a3t 3 +a4t 4 +a5t 5 The coefficients are determined by the boundary conditions (the position, velocity, and acceleration of the starting and ending points are all zero), the trajectory update period is ≤150ms, and the joint acceleration is ≤10rad / s². 2 .

[0246] In step S4, the dynamic visual calibration includes two stages: initial static calibration and dynamic incremental compensation.

[0247] Initial static calibration: achieved by combining PnP algorithm with RANSAC robust estimation. 40 sets of images were acquired in 8 different poses using the ArUco calibration board. The transformation relationship between the visual sensor's 24 coordinate system and the world coordinate system was solved, and the initial reprojection error was <0.3 pixels. The coordinate systems of the dual-arm base were aligned by sharing calibration points through the end effector 3 contacts to ensure uniform dual-arm pose.

[0248] Dynamic incremental compensation: Dual-thread parallel processing is adopted: Thread 1 performs coarse compensation by measuring the displacement Δd using a laser rangefinder (BenewakeTF03), which takes ≤5ms; Thread 2 acquires images using the Realsense D455i vision sensor 24 and optimizes the pose residual ΔTerror using the Levenberg-Marquardt algorithm for fine correction, which takes ≤20ms.

[0249] The total cycle of dynamic visual calibration is ≤25ms. The ROS multi-threaded callback mechanism is used to achieve thread scheduling, resulting in low CPU utilization. After dynamic compensation, the positioning error is ≤1mm.

[0250] The dual-thread parallel processing solves large-scale displacement through laser coarse compensation and visual fine-tuning eliminates calibration drift caused by environmental interference (such as changes in lighting and shading by branches), reducing the total time of dynamic calibration compared to single-thread, and meeting the real-time requirements of the robotic arm 2 at a maximum lifting speed of 0.3m / s.

[0251] Initial static calibration establishes a high-precision benchmark and reduces cumulative errors in dynamic compensation: By combining the PnP algorithm with RANSAC robust estimation, 40 sets of images were acquired using the ArUco calibration board in 8 different poses to solve the initial transformation relationship between the vision sensor 24 and the world coordinate system. The initial reprojection error was <0.3 pixels, providing a high-precision benchmark for dynamic incremental compensation. At the same time, the coordinate system alignment of the two arms bases was achieved by contacting the shared calibration point of the end effector 3, avoiding the cooperative error caused by the coordinate system deviation of the two arms, and reducing the initial error accumulation of dynamic calibration from the source.

[0252] The layered compensation mechanism balances speed and accuracy, adapting to complex orchard environments: The dynamic incremental compensation adopts a layered strategy of "laser coarse compensation + visual fine correction": The laser rangefinder (BenewakeTF03) quickly measures the displacement Δd (time ≤ 5ms), solving the problem of large-scale rigid displacement caused by the lifting of the robotic arm 2; The vision sensor 24 (RealsenseD455i) optimizes the pose residual ΔTerror through the Levenberg-Marquardt algorithm (time ≤ 20ms), accurately eliminating calibration drift caused by environmental interference such as changes in lighting and branch shading, achieving the dual advantages of "fast response + accurate correction".

[0253] Strict real-time and accuracy indicators ensure harvesting reliability: the total cycle of dynamic visual calibration is ≤25ms, and the ROS multi-threaded callback mechanism achieves efficient thread scheduling, ensuring the real-time response of the robotic arm 2 at a maximum lifting speed of 0.3m / s; the positioning error after dynamic compensation is ≤1mm, which meets the core indicator of "reducing positioning error (≤1mm) to meet the harvesting accuracy requirements" in claim 1, and provides coordinate mapping guarantee for the end effector 3 (robotic claw) to accurately grasp the target fruit.

[0254] The following describes the initial calibration procedure, calibration board type, and error indicators.

[0255] I. Operating Procedures.

[0256] 1. Calibration preparation: Keep the mobile platform 1 stationary and horizontal, and fix the ArUco calibration plate at a known position in the world coordinate system {W} as the calibration reference.

[0257] Activate the dynamic vision calibration module to ensure that the RealSense D455i vision sensor 24 and the system are in a stable working state.

[0258] 2. Image acquisition: The vision sensor 24 is controlled to acquire images of the stationary ArUco calibration board from 8 different poses. Five sets of images are acquired in each pose, for a total of 40 sets of valid image data, covering the complete working field of view of the vision sensor 24.

[0259] 3. Coordinate Transformation Solution: For each set of images, the pixel coordinates of the corner points of the ArUco calibration board are extracted. Combined with the known 3D world coordinates of the corner points, the transformation relationship Tc→W from the 24-coordinate system of the vision sensor to the world coordinate system is solved using the PnP algorithm (perspective n-point algorithm). The RANSAC robust estimation algorithm is used to remove outliers to ensure the stability of the transformation matrix.

[0260] The specific method is as follows: For each set of images, extract the pixel coordinates of the ArUco-marked corner points. Combined with the known 3D world coordinates of the corner points on the calibration board, use the perspective n-point algorithm (PnP) to solve for the camera's extrinsic matrix, i.e., the projection matrix.

[0261]

[0262] Where K is the known camera intrinsic parameter matrix. Let the coordinates of the calibration board corner points in the world coordinate system {W} be {Xw, Yw, Zw}. When applying the perspective n-point algorithm to solve for the camera extrinsic parameters, the RANSAC robust estimation algorithm is used to remove matching outliers, ensuring the robustness of the extrinsic parameter estimation. The average reprojection error of the final calibration result is less than 0.3 pixels, meeting the accuracy requirements.

[0263] 4. Align the coordinate systems of both arms.

[0264] The end effector 3 (mechanical gripper) of the dual robotic arms 2 is controlled to contact the preset shared calibration point, read the joint encoder data of the dual arms, and calculate the transformation relationship from the left arm base coordinate system to the right arm base coordinate system through forward kinematics to achieve uniform posture of the dual arms.

[0265] The base of the left robotic arm 2 to the world coordinate system Transformation relationship. Let the coordinates of the end contact point of the robotic arm 2 at the i-th corner point in the base coordinate system be... in The coordinates are obtained by solving the forward kinematics through the joint angles. The coordinates of the contact point in the world coordinate system are known. The optimal transformation is obtained by least squares fitting:

[0266]

[0267] The reference transformation matrix is ​​synthesized as follows:

[0268]

[0269] It can be deduced that:

[0270]

[0271] After unfolding, you can obtain

[0272]

[0273] This yields the transformation relationship between the camera coordinates and the coordinates of robotic arm 2. Then, the coordinate systems of the two arms are aligned by controlling the left and right end effectors 3 of robotic arm 2 to contact the shared calibration point. Read the biarm joint angle θ L ,θ R The coordinate transformation relationship from the left arm to the right arm base, calculated using forward kinematics, is as follows:

[0274]

[0275] II. Calibration plate type.

[0276] ArUco Calibration Board: A planar calibration board containing coded markers to provide high-precision corner features and support the solution of spatial mapping relationships between vision sensors 24 and the world coordinate system.

[0277] III. Error Indicators.

[0278] Initial reprojection error: ≤0.3 pixels (based on the average error of 40 sets of images), ensuring the initial mapping accuracy between the visual sensor's 24 coordinate system and the world coordinate system.

[0279] Dual-arm pose alignment error: This is achieved by using the end effector 3 to contact the shared calibration point, providing a benchmark for coordinate mapping correction in subsequent dynamic visual calibration, and indirectly ensuring the accuracy requirement of positioning error ≤1mm after dynamic compensation.

[0280] The initial calibration provides a high-precision benchmark for the "dynamic incremental compensation" (laser coarse compensation + visual fine correction) of the dynamic vision calibration module, which is a prerequisite for achieving "calibration cycle ≤ 25ms" and "positioning error ≤ 1mm".

[0281] The combination of the PnP algorithm and RANSAC ensures the robustness of the visual sensor 24 in complex orchard environments (light variations, branch occlusion), and supports the fruit recognition and branch extraction modules to output accurate three-dimensional coordinates of the target fruit.

[0282] IV. Dynamic Incremental Compensation Calibration.

[0283] 1. Dynamic incremental compensation process.

[0284] During the lifting and lowering process of robotic arm 2, the dynamic vision calibration module achieves real-time correction of coordinate mapping relationships through dual-thread parallel processing of "laser coarse compensation + vision fine correction". The specific steps are as follows:

[0285] 1.1 Laser Coarse Compensation: The lifting displacement Δd of robotic arm 2 is measured using a Benewake TF03 laser rangefinder, generating a rigid translation transformation matrix TΔd. This matrix is ​​used to pre-compensate the pose of the origin of the base coordinate system in the world coordinate system, yielding the estimated pose of the base coordinate system. Taking the left arm as an example, if the left arm platform is raised, the rigid translation transformation matrix generated based on the displacement of the origin of the base coordinate system in the world coordinate system is as follows:

[0286] Predict the pose of the left arm in the base coordinate system:

[0287]

[0288] 1.2 Visual refinement compensation was performed. RealSense D455 was used to acquire images of the calibration board in real time and extract the coordinates P of ArUco corner points. actual Calculate the vision-based pose residual matrix:

[0289]

[0290] in To determine the theoretical coordinates of the feature points on the calibration board, the Levenberg-Marquardt algorithm is used to optimize the solution of ΔT. error

[0291]

[0292] Laser coarse compensation takes ≤5ms (rangefinder model: BenewakeTF03), and visual fine compensation takes ≤20ms (RealsenseD455i). This is achieved through dual-thread parallel processing: thread 1 performs laser displacement calculation, and thread 2 performs visual residual optimization. The total calibration cycle is ≤25ms.

[0293] As an optimized implementation of the calibration system, this invention also provides a segmented pre-storage architecture and a dynamic hierarchical strategy.

[0294] Segmented pre-stored architecture: By pre-storing static calibration packages for key locations (such as 200 / 400 / 600mm), the coordinate mapping relationship can be directly called, reducing the initial calculation time of dynamic compensation and supporting the real-time requirement of "calibration cycle ≤ 25ms".

[0295] Dynamic grading strategy: When the displacement is not in the pre-stored position, incremental compensation (laser coarse compensation + visual fine correction) is triggered. The residual is optimized by the Levenberg-Marquardt algorithm to ensure the accuracy index of "positioning error ≤ 1mm".

[0296] In step S5, when the flexible-rigid composite actuator contacts the fruit, the force sensor feedback stress reaches 15 kPa, triggering the pneumatic cutting blade to move. The cutting force is adaptively adjusted according to the diameter of the fruit stem (range 0.5-2 N).

[0297] Execution parameters: cutting blade thickness 0.3mm, air source pressure 0.4-0.6MPa, response time ≤20ms.

[0298] Avoid excessive force that could damage the fruit (skin damage rate <0.5%), and adapt to the stem hardness of different apple varieties (such as Fuji and Gala).

[0299] The industrial control computer has a segmented pre-stored architecture and a dynamic hierarchical strategy;

[0300] Pre-stored architecture: Based on the initial static calibration results, calibration parameter packages are pre-stored at vertical displacements of 200 / 400 / 600mm. This is a reference data preparation step for dynamic visual calibration. The coordinate mapping relationship is directly called to reduce the initial calculation time of dynamic compensation and to quickly call the calibration results to reduce the amount of real-time calculation.

[0301] Dynamic grading strategy: When the actual displacement exceeds the pre-stored position, incremental compensation (laser coarse compensation + visual fine correction) is triggered. The residual is optimized through the Levenberg-Marquardt algorithm to ensure a positioning error of ≤1mm. This belongs to the "real-time compensation" stage of dynamic visual calibration, which is a dynamic adaptation and optimization of the calibration process.

[0302] The above embodiments are for illustrative purposes only and are not intended to limit the technical solutions of the present invention. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the present invention without departing from the spirit and scope of the present invention. For example, a linear module (ball screw) can be used to drive the robotic arm 2 to lift independently (stroke 0-1.2m, positioning accuracy ±0.5mm) instead of a scissor lifting unit; a SIFT feature point matching algorithm can be used for pure visual calibration with a calibration period ≤50ms, which is suitable for scenarios where the speed of the mobile platform 1 is ≤0.3m / s, to replace dynamic visual calibration. All such transformations should be covered within the scope of the claims of the present invention.

Claims

1. A dual six-degree-of-freedom variable workspace robotic arm, characterized in that: It includes a mobile platform, two 6-DOF robotic arms, a scissor lift unit that drives each robotic arm to lift independently, an end effector, and a control system that integrates a fruit recognition and branch extraction module, a dynamic visual calibration module, and a dual-arm collaborative control module. The scissor lifting unit is connected to the servo motor drive screw through an independent X-shaped cross hinge mechanism to achieve single-arm vertical lifting; The fruit recognition and branch extraction module is used to obtain the three-dimensional coordinates of the target fruit and the topological structure of the branch; The dynamic vision calibration module uses a dual-thread parallel processing of a laser rangefinder and a vision sensor to correct the coordinate mapping relationship in the movement of the robotic arm in real time. The dual-arm collaborative control module uses the target fruit's three-dimensional coordinates and branch topology output by the fruit recognition and branch extraction module to achieve dynamic task allocation and safe path planning for both arms.

2. The dual six-degree-of-freedom variable workspace robotic arm according to claim 1, characterized in that: The dual-arm collaborative control module includes a dynamic task allocation unit and an agronomic feature fusion path planning unit; the dynamic task allocation unit allocates fruit picking tasks based on a spatiotemporal benefit cost function, which is: Cost k It is the total cost of the k-th robotic arm picking a certain fruit, which is dimensionless; α = 0.6, β = 0.3, γ = 0.1; α is the time cost weighting coefficient, which is dimensionless; β is the collision risk weighting coefficient, which is dimensionless; γ is the dimensionless coefficient of the high-fit penalty weight. armk refers to the current position coordinates of the origin of the base coordinate system of the robotic arm k, in meters; "Fruit" refers to the three-dimensional coordinates of the target fruit, that is, the spatial position of the apple to be picked by the robotic arm in the world coordinate system, in meters, and is output by the fruit recognition and branch extraction module. v max It is the maximum permissible linear velocity of the robotic arm, measured in meters per second. It is an inherent parameter that existed prior to this invention and is determined by the hardware of the robotic arm, belonging to the physical limitations of the equipment. Risk collision It is a collision risk score on the picking path, which is dimensionless and is a design parameter of this invention. It is obtained by weighted integral of the distance between all points on the path and the nearest branch or normalized by the maximum violation value. Height penalty It is a highly adaptive penalty term, dimensionless, and is a design parameter of this invention. (Height) penalty =EXP(|Harm-Hfruit|), where Harm is the current height of the end effector of the robotic arm in meters, and is the status feedback data of the robotic arm; Hfruit represents the height of the target fruit in meters, which is extracted by the fruit recognition and branch extraction module from the height coordinate zi in the three-dimensional coordinate set Pf = (xi, yi, zi) output by the YOLOv5s model and the visual sensor. The path planning unit sets a safety distance based on the branch diameter: 50 mm when the branch diameter is ≥15 mm and 30 mm when the branch diameter is <15 mm.

3. The dual six-degree-of-freedom variable workspace robotic arm according to claim 1, characterized in that: The end effector is a flexible-rigid composite structure, including a silicone elastic contact layer, an integrated force sensor, and a miniature pneumatic cutting blade with a cutting force ≤2N.

4. The dual six-degree-of-freedom variable workspace robotic arm according to claim 1, characterized in that: The scissor lift unit includes a bottom frame fixed to the upper surface of the mobile platform and a lift platform. The bottom surface of the lift platform is provided with an upper frame, which is located directly above the bottom frame. The bottom frame is connected to two parallel and spaced lower rails, and the top frame is connected to two parallel and spaced upper rails. The upper rails and lower rails correspond one-to-one, and the corresponding upper and lower rails are connected by an X-shaped cross hinge device. Each X-type cross hinge device includes two hinge plates, which are hinged together at the middle by a central axis, and the two hinge plates form an X-shaped cross structure with the central axis as the center. One bottom end of the hinge plate rotates downward and engages with the lower hinge short shaft. The lower hinge short shaft passes through the lower hinge seat and is hinged to the lower hinge seat. The lower hinge seat is fixedly connected to the lower rail. The lower hinge short shaft, the lower hinge seat, and the lower rail correspond one-to-one. The other bottom end of the hinge plate rotates downward and engages with the moving shaft. A lower moving wheel is installed on the moving shaft and rolls with the lower rail. The top edge of one side of the hinge plate rotates upward and engages with the upper hinge short shaft. The upper hinge short shaft passes through the upper hinge seat and is hinged to the upper hinge seat. The upper hinge seat is fixedly connected to the upper rail. The upper hinge short shaft, the upper hinge seat, and the upper rail correspond one-to-one. The top edge of the other side of the hinge plate rotates upward and engages with the upper moving wheel. The upper moving wheel rolls with the upper rail. The lower hinge short shaft, lower hinge seat, lower rail and lower moving wheel correspond one-to-one with the upper hinge short shaft, upper hinge seat, upper rail and upper moving wheel; The two sets of X-type cross hinge devices share the same moving axis and the same central axis. The base frame is connected to a servo motor, and the output shaft of the servo motor is connected to a lead screw via a coupling. A transmission block is fixedly connected to the middle of the moving shaft, and the middle of the lead screw passes through the transmission block and is threaded into the transmission block. The other end of the lead screw is connected to the base frame via a bearing seat. Two sets of X-type cross hinge devices form an X-type cross hinge mechanism.

5. The dual six-degree-of-freedom variable workspace robotic arm according to claim 1, characterized in that: The scissor lifting unit includes two sets of independent X-shaped cross hinges, which are hinged by a central pin. The servo motor drives the lead screw to push the bottom corner of the hinge to achieve extension and retraction. The single arm lifting speed is 0.1~0.3m / s, and the stroke is 0-1.2m. The scissor lift unit features a height difference constraint and dynamic compensation mechanism for the two arms, specifically: The height difference between the two arms of the scissor lift unit is constrained to |H1-H0|≤800mm; when the actual height difference is detected to exceed the threshold, a dynamic compensation mechanism is triggered: the lifting speed of the lagging arm is increased by 1.2 times until the height difference is ≤600mm and then the nominal speed is restored.

6. The dual six-degree-of-freedom variable workspace robotic arm according to claim 1, characterized in that: The control system is an industrial control computer integrated into the mobile platform. It realizes joint status acquisition, trajectory planning and actuator control through ROS nodes. It communicates with the robotic arm via cable, and the response delay is ≤20ms.

7. A dual-arm collaborative harvesting control method using the dual six-degree-of-freedom variable workspace robotic arm as described in claim 2, characterized in that, Includes the following steps: S1. Fruit recognition and branch extraction: The three-dimensional coordinates of the fruit are identified by the YOLOv5s model, and the topology and diameter parameters of the branches are extracted by the PointNet++ network. S2. Dynamic task allocation: Calculate the dual-arm harvesting cost based on the spatiotemporal benefit cost function, and use the Hungarian algorithm to output the fruit-robotic arm mapping relationship; S3. Agronomic Path Planning: Based on the branch diameter classification, safety distances are set. An obstacle avoidance trajectory is generated using an improved RRT* algorithm. In the improved RRT* algorithm, branches with a diameter ≥15mm are marked as rigid branches and a safety distance of 50mm is set (hard constraint), and path points must be greater than or equal to 50mm. Branches with a diameter <15mm are marked as flexible branches and a safety distance of -30mm is set (soft constraint), allowing path points to penetrate elastically by less than 30mm. That is, when penetrating, the robotic arm is allowed to push the flexible branch to produce elastic deformation within 30mm. S4. Dynamic visual calibration: When the robotic arm is raised and lowered, the laser rangefinder measures the displacement Δd for coarse compensation, and simultaneously optimizes the visual pose residual ΔTerror through the Levberg-Marquardt algorithm. S5. End-effector control: The flexible-rigid composite actuator adjusts the gripping force based on feedback from the force sensor, and the pneumatic cutter cuts the fruit stem.

8. The method according to claim 7, characterized in that: In step S3, the improved RRT* algorithm uses a sampling density three times that of the region >500mm from the fruit within a range of <200mm. The trajectory is smoothed using fifth-order polynomial interpolation, and the joint acceleration is ≤10 rad / s². 2 .

9. The method according to claim 7, characterized in that: In step S4, the dynamic visual calibration includes two stages: initial static calibration and dynamic incremental compensation. Initial static calibration: achieved by combining PnP algorithm with RANSAC robust estimation. 40 sets of images were acquired in 8 different poses using the ArUco calibration board. The transformation relationship between the visual sensor coordinate system and the world coordinate system was solved, and the initial reprojection error was <0.3 pixels. The coordinate systems of the dual-arm base were aligned by the end effector contacting the shared calibration point to ensure uniform pose of the dual arms. Dynamic incremental compensation: adopts dual-thread parallel processing: Thread 1 performs coarse compensation by measuring the displacement Δd through a laser rangefinder, with a time of ≤5ms; Thread 2 acquires images through a vision sensor and uses the Levenberg-Marquardt algorithm to optimize the pose residual ΔTerror for fine correction, with a time of ≤20ms.

10. The method according to claim 7, characterized in that: The industrial control computer has a segmented pre-stored architecture and a dynamic hierarchical strategy; Pre-stored architecture: Based on the initial static calibration results, calibration parameter packages are pre-stored at vertical displacements of 200 / 400 / 600mm. The coordinate mapping relationship is directly called to reduce the initial calculation time of dynamic compensation and to quickly call the calibration results to reduce the amount of real-time calculation. Dynamic grading strategy: When the actual displacement exceeds the pre-stored position, incremental compensation is triggered, and the residual is optimized by the Levenberg-Marquardt algorithm to ensure the positioning error is ≤1mm.

Images