An automatic passion fruit identification and harvesting system
By constructing a joint recognition mechanism for fruit and stem and camera calibration, combined with depth data repair and closed-loop control of the robotic arm, the problem of target recognition and cutting point positioning of passion fruit picking robots in complex environments was solved, realizing stable, accurate and automated picking of passion fruit.
Patent Information
- Application Number
- CN202611578905.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-10-13
- Publication Date
- 2026-11-17
AI Technical Summary
Existing passion fruit harvesting robots lack stability in target recognition under complex trellis environments. Depth data is easily affected by natural light, the correspondence between fruit and stem is unclear, the positioning error of the cutting point is large, and the reliability of the robotic arm control is insufficient. It is difficult to achieve stable and accurate three-dimensional cutting in environments with intertwined branches and vines, small shading by stems, target swaying with the wind, and significant changes in lighting.
A joint recognition mechanism for fruit and pedicel is constructed. The depth information in the target neighborhood is used for adaptive screening and repair. Through camera calibration and robotic arm coordinate system transformation, continuous multi-frame target matching and spatial position stability judgment are introduced. The target recognition result and depth data quality are comprehensively judged to carry out robotic arm motion planning and closed-loop control to ensure that the robotic arm reaches the cutting position safely and accurately.
It improves the accuracy and stability of passion fruit picking positioning, realizes automated picking in complex natural environments, reduces positioning lag and shearing deviation of traditional methods under fruit swing conditions, and improves the reliability of robotic arm control and operational safety.
Smart Images

Figure CN123100398A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the fields of smart agricultural equipment, machine vision and robot control technology, and specifically relates to an automatic passion fruit identification and harvesting system. Background Technology
[0002] Passion fruit (Passiflora edulis) is a tropical and subtropical fruit with high economic value. As its cultivation scale expands, its harvesting operations are increasingly reliant on labor. Currently, passion fruit harvesting is still primarily manual, requiring workers to continuously search for ripe fruit under trellises, holding, cutting stems, and collecting them. This method suffers from high labor intensity, low efficiency during continuous operation, high labor costs, and the harvest quality being significantly affected by the worker's skill level. During the peak ripening period, insufficient manual harvesting capacity has become a major factor restricting large-scale cultivation.
[0003] In recent years, machine vision, deep learning, and robot control technologies have developed rapidly. Harvesting robots for common fruits and vegetables such as apples, tomatoes, and citrus have formed a basic technical approach encompassing target recognition, spatial localization, robotic arm approach, and end effector execution. Existing apple harvesting robots typically use RGB or depth images to identify the fruit and control the robotic arm to cut it based on the fruit's center position. These robots are suitable for scenarios where the fruit outline is relatively clear and the harvesting target is primarily the fruit itself. However, their ability to identify small fruit stems and determine the cutting position is limited. Tomato harvesting robots typically identify both the fruit and the stem simultaneously and use a gripping or shearing end effector to complete the harvest. They can achieve stem localization to some extent, but their adaptability to complex natural light conditions, dense foliage shading, and dynamic swaying of the target remains insufficient, especially in greenhouse environments with relatively stable lighting and regular planting structures. Passion fruit primarily grows in tropical trellis environments, where branches, vines, fruits, and supports intertwine. The fruit stalks are long and easily obstructed, resulting in significant variations in target size. In outdoor or semi-open environments, changes in natural light, shooting angle, and infrared background radiation can easily cause localized failures in depth images, producing depth holes or outliers, leading to deviations in the 3D coordinates of the shearing point. Research on passion fruit harvesting robots is relatively limited compared to other fruits and vegetables. Existing harvesting methods often focus on fruit detection, neglecting the relationship between the fruit and stalk, stalk orientation, and optimal shearing positions, making it difficult to provide stable and accurate 3D shearing points for shearing-type end effectors. Furthermore, wind, vine swaying, and robot vibrations can cause dynamic changes in the target position. If harvesting is performed solely based on single-frame detection results and single depth values, it is susceptible to momentary false detections, depth anomalies, and target swaying, resulting in positioning drift, shearing misalignment, or collisions. Since the camera and robotic arm operate in different coordinate systems, using empirical offset methods for coordinate conversion makes it difficult to adapt to changes in camera installation position and working distance, easily leading to systematic positioning errors. Furthermore, the robotic arm's movement must also consider joint constraints, environmental obstacles, and the approach direction of the end effector; fixed trajectories or simple linear motion cannot simultaneously guarantee both efficiency and operational safety. Therefore, existing fruit and vegetable harvesting robots and related technologies cannot directly meet the automated harvesting needs of passion fruit in complex trellis environments. They still have shortcomings in areas such as depth data repair under complex lighting conditions, joint recognition of fruit and stem, 3D positioning of cutting points, vision-robotic arm coordinate transformation, target stability assessment, and robotic arm safety control. There is an urgent need for a passion fruit harvesting robot that can combine target perception, depth processing, coordinate calibration, stability assessment, and closed-loop control of the robotic arm to improve the accuracy, reliability, and automation of harvesting positioning in complex trellis environments. Summary of the Invention
[0004] The purpose of this invention is to address the problems of existing passion fruit harvesting robots in complex trellis environments, including insufficient target recognition stability, susceptibility of depth data to natural light interference, unclear correspondence between fruit and stem, large errors in shearing point positioning, and insufficient reliability of robotic arm control. Particularly in actual working environments with intertwined branches and vines, thin stems providing obstruction, targets swaying in the wind, and significant variations in light intensity, depth cameras are prone to producing depth holes or outliers. Traditional methods, relying solely on single-frame detection results and single depth values to determine the harvesting position, struggle to obtain stable and accurate three-dimensional shearing points. Furthermore, using empirical offsets for visual coordinate to robotic arm coordinate conversion is susceptible to systematic errors due to variations in camera installation position, working distance, and mechanical structure, leading to shearing misalignment, unreasonable movement trajectories, or collisions, thus affecting the accuracy and stability of the harvesting operation.
[0005] This invention establishes a spatial relationship between the fruit and its corresponding stem by constructing a joint recognition mechanism, and determines a reasonable cutting point based on the stem's position and orientation. To address depth holes and abnormal depth values caused by natural light interference, it adaptively filters and repairs depth data by combining effective depth information within the target's neighborhood to obtain reliable cutting point depths. Through camera calibration and camera-robotic arm coordinate system transformation, the two-dimensional cutting point in the image is converted into a three-dimensional target position in the robotic arm coordinate system. Simultaneously, a continuous multi-frame target matching and spatial position stability judgment mechanism is introduced to comprehensively assess the target recognition result, depth data quality, and the degree of target position change, triggering the harvesting operation only when the target meets stability conditions. Based on this, motion planning and closed-loop control are performed by combining robotic arm joint constraints, environmental obstacles, and the end effector's approach direction, enabling the robotic arm to safely and accurately reach the cutting position and complete the harvest, thereby improving the accuracy, stability, and automation of passion fruit harvesting positioning in complex natural environments.
[0006] To achieve the above objectives, the present invention provides the following solution: an automatic passion fruit identification and harvesting system, comprising: Calibration and initialization module: calibrates the camera intrinsic parameters, distortion parameters, and fixed hand-eye transformation matrix, and initializes the camera intrinsic parameters, distortion parameters, fixed hand-eye transformation matrix, robotic arm joint limits of the picking robot, end effector installation offset, effective range of camera, and control cycle; Image acquisition and processing module: The visual acquisition module of the harvesting robot acquires RGB images and original depth maps of passion fruit and stems in the working area; and performs depth quality judgment based on the RGB images and the original depth map to obtain depth data; the depth data includes: effective depth mask, candidate region depth effectiveness rate, completed depth map, and shear point depth; Recognition module: Based on the depth data, the module identifies and associates the fruit and pedicel, and based on the association between the fruit and pedicel, obtains the fruit-pedicel connection point, shearing point and fruit tracking reference point of the current frame. Coordinate recovery module: Based on the fruit-stalk connection point, the fruit tracking reference point, and the shearing point in the current frame, combined with the camera intrinsic parameters, the shearing point depth, and the fixed hand-eye transformation matrix, the module recovers the three-dimensional coordinates of the fruit-stalk connection point, the fruit tracking reference point, and the shearing point in the camera coordinate system and the robot arm base coordinate system; it determines the spatial orientation of the fruit stalk in the robot arm base coordinate system based on the three-dimensional positions of the connection point and the shearing point, and establishes the target geometric constraints required for dynamic approach and clamping / shearing operations; Motion state estimation module: Based on the three-dimensional coordinates and the candidate region depth, it efficiently establishes motion state estimation models for the fruit tracking reference point and the shearing point; the motion state estimation model includes: state vector, state transition equation, observation vector, and observation equation; through state prediction and visual observation updates, it outputs the target image motion speed, target spatial motion speed, and state prediction covariance; based on the timestamp and feedback from the robotic arm controller, it determines the prediction compensation time and predicts the shearing point state at the actual execution time of the control command, thereby obtaining the predicted shearing point; Target pose update module: Converts the predicted shear point into normalized image coordinates and establishes the predicted image feature error; extracts the rotation control sub-matrix based on the image interaction matrix of the target point, and then combines the predicted image feature error to obtain the camera angular velocity control quantity; when the image error enters the set range, it generates the approach velocity in the direction of the camera optical axis based on the error between the predicted shear point depth and the desired shear depth; obtains the visual servo velocity vector in the camera coordinate system from the camera angular velocity control quantity and the approach velocity in the direction of the camera optical axis, and then obtains the new target pose of the camera and the new target pose of the robotic arm end effector. Dynamic shearing pose estimation module: Based on the predicted shearing point coordinates and predicted depth, calculates the target's three-dimensional position at the moment of shear closure and transforms it to the robot arm's base coordinate system; constructs the dynamic shearing target pose based on the predicted shearing point, fruit stem direction, and end effector installation offset; when the predicted image feature error, depth error, target motion speed, depth efficiency, and robot arm status simultaneously meet the set conditions, and the set conditions are continuously maintained for a set number of frames, the fruit-fruit stem correlation remains consistent, the predicted shearing point is located within the robot arm's allowed working space, and the robot arm has no collision risk or malfunction, dynamic shearing is allowed; otherwise, returns to the target pose update module, or stops approaching and returns to the pre-shearing safe pose; Dynamic shearing execution module: Establishes a kinematic model of the robotic arm based on the dynamic shearing target pose, and then solves the joint variables of the robotic arm; when the shearing conditions are met continuously for a set number of frames, controls the end shears to close and cut the fruit stem, transferring the fruit to the placement area.
[0007] Loop tracking module: When a target is lost, movement exceeds limits, communication is abnormal, or actuator malfunctions, the control robot arm stops its current action, returns to a safe position, or re-enters the target detection and tracking process. After the current target is successfully picked, the target status is updated and the picking process for the next target to be picked begins.
[0008] More preferably, the fixed hand-eye transformation matrix includes: ; In the formula, R EC This is the rotation matrix from the camera coordinate system to the robot arm end effector coordinate system; t EC is the position vector of the origin of the camera coordinate system in the coordinate system of the end effector of the robotic arm; r ij For the camera coordinate system j The coordinate axis is related to the coordinate system of the robotic arm's end effector. i The cosine of the angle between the coordinate axes; t x , t y , t z These represent the positions of the robotic arm's end effector along... x , y , z Components of axis translation This is a fixed hand-eye transformation matrix from the camera coordinate system to the end effector coordinate system of the robotic arm.
[0009] More preferably, for ,area The depth efficiency is: ; In the formula, c The subscript indicating the cut point; Indicates the first k The proportion of effective pixels in the candidate region of each shearing point to the total number of pixels; Candidate region for shear point The first 1 pixel; This represents the total number of pixels within the candidate cropping region. M k ( ) indicates the first k Frame pixels p The original depth validity identifier; The shear point depth includes: ; In the formula, This represents the set of depth values that meet the criteria of effective ranging range and neighborhood outlier filtering. The depth of the shear point; Indicates the first k After the frame is effectively depth preserved and missing depth is filled, the pixels p The corresponding depth value.
[0010] More preferably, the method for obtaining the predicted shear point includes: ; In the formula, These represent the x-coordinate, y-coordinate, and depth of the predicted shear point image, respectively. This represents the current state observation value of the shear point. This represents the rate of change corresponding to the observed state value at the shear point at the current moment; This refers to the latency of visual processing and control command execution.
[0011] More preferably, the new target pose of the camera for: ; In the formula, This represents the pose homogeneous transformation matrix of the current camera coordinate system relative to the robot arm's base coordinate system; This represents the pose increment homogeneous transformation matrix expressed in the current camera coordinate system within a control cycle; and the new target pose at the end of the robotic arm for: ; In the formula, This represents the homogeneous pose transformation matrix of the current end-effector coordinate system relative to the robot arm base coordinate system.
[0012] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention combines RGB-D target perception, three-dimensional spatial positioning, target motion state prediction, and IBVS dynamic visual servoing, enabling the robotic arm to continuously adjust its end-effector trajectory based on the motion state of the passion fruit. This reduces the positioning lag and shearing deviation caused by traditional single-target positioning methods under fruit swinging conditions. At the same time, through shearing condition determination, CAN closed-loop execution, and anomaly feedback mechanisms, the stability of the robotic arm's approach, gripping, and shearing processes is improved, achieving continuous dynamic tracking and automatic harvesting of passion fruit under natural swinging conditions. This meets the actual needs of automated passion fruit harvesting operations in orchard environments. Attached Figure Description
[0013] To more clearly illustrate the technical solution of the present invention, the drawings used in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0014] Figure 1 This is a schematic diagram of the passion fruit target perception and motion state prediction stage and the IBVS dynamic tracking and closed-loop harvesting stage provided in an embodiment of the present invention. Figure 2 This is a schematic diagram of the passion fruit orchard environment and orchard harvesting provided in an embodiment of the present invention; wherein, (a) is a real photograph of the passion fruit orchard harvesting environment; (b) is a diagram of the passion fruit robotic arm harvesting platform; and (c) is a diagram of the robotic arm operating scissors for harvesting. Figure 3 The present invention provides target localization segmentation and depth map in an embodiment of the invention; wherein, (a) is target localization segmentation; and (b) is depth map; Figure 4 This is a line diagram of a robotic arm harvesting experiment provided in an embodiment of the present invention. Detailed Implementation
[0015] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0016] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0017] Example 1: like Figure 1 As shown, this embodiment provides an automatic passion fruit identification and harvesting system, including: a calibration and initialization module: calibrating the camera intrinsic parameters, distortion parameters, and fixed hand-eye transformation matrix, and initializing the camera intrinsic parameters, the distortion parameters, the fixed hand-eye transformation matrix, the mechanical arm joint limits of the harvesting robot, the end effector installation offset, the effective ranging range of the camera, and the control cycle.
[0018] The visual acquisition module of the harvesting robot adopts an "eye-on-hand" mounting method, with the RGB-D camera and the end effector of the robotic arm maintaining a fixed, rigid connection. A checkerboard calibration board is used to calibrate the camera's intrinsic parameters, obtaining the equivalent focal length of the camera in the horizontal and vertical directions. Principal point coordinates And distortion parameters.
[0019] Camera intrinsic matrix K for: (1) This embodiment specifies This represents the homogeneous transformation matrix that transforms coordinates in coordinate system 2 to coordinate system 1. During hand-eye calibration, the checkerboard calibration plate is fixedly set within the robotic arm's workspace, and the robotic arm is controlled to reach... Different positions and postures. In the first... Under pose conditions, the robotic arm controller obtains the end effector coordinate system. Relative to the robot arm's base coordinate system Transformation matrix The visual acquisition module obtains the calibration board coordinate system based on the corner points of the chessboard. G To the camera coordinate system Transformation matrix .
[0020] The coordinate systems satisfy the following: (2) In the formula, This is a fixed hand-eye transformation matrix from the camera coordinate system to the robotic arm end effector coordinate system; This is a fixed transformation matrix from the calibration plate coordinate system to the robot arm base coordinate system.
[0021] Select the first i Group and No. j Assemble the target pose and construct the relative motion matrix: (3) make The hand-eye labeling relationship is then represented as: (4) In the formula, A ij and B ij These represent the relative motion transformation matrix of the robotic arm end effector and the relative motion transformation matrix of the camera, respectively.
[0022] Solve the fixed hand-eye transformation matrix based on multiple sets of relative motion data: (5) In the formula,X * represents the estimate of the fixed hand-eye transformation matrix that minimizes the sum of squared residuals from multiple sets of relative motion relationships.
[0023] The obtained fixed hand-eye transformation matrix is represented as follows: ; (6) In the formula, R EC This is the rotation matrix from the camera coordinate system to the robot arm end effector coordinate system; t EC is the position vector of the origin of the camera coordinate system in the coordinate system of the end effector of the robotic arm; r ij For the camera coordinate system j The coordinate axis is related to the coordinate system of the robotic arm's end effector. i The cosine of the angle between the coordinate axes; t x , t y , t z These represent the positions of the robotic arm's end effector along... x , y , z The component of axis translation.
[0024] During the operation of the robotic arm, the end-effector pose is fed back in real time by the robotic arm controller. T BE Calculate the real-time transformation matrix from the camera coordinate system to the robot arm base coordinate system. T BC : (7) After calibration, the camera intrinsic parameters, distortion parameters, fixed hand-eye transformation matrix, robotic arm joint limits, end effector installation offset, camera effective ranging range, and control cycle are initialized.
[0025] Image acquisition and processing module: The visual acquisition module of the harvesting robot acquires RGB images and original depth maps of passion fruit and stems in the working area; and performs depth quality judgment based on the RGB images and the original depth map to obtain depth data; the depth data includes: effective depth mask, candidate region depth effectiveness rate, completed depth map and shear point depth.
[0026] The visual acquisition module synchronously acquires RGB images of the work area. and the original depth map During initial detection, depth quality is assessed within a pre-defined working area. During continuous tracking, candidate regions are established based on the clipping points predicted in the previous frame. After target recognition is complete, the corresponding depth quality metrics are updated using the actual detection area. Within the pre-defined working area, the main control module uses the camera's minimum effective ranging value... and maximum effective distance measurement value Establish a depth-effective mask : (8) When the original depth value is zero, a non-finite value, or exceeds the effective ranging range, let The shear point candidate regions and the shear point candidate regions are respectively used as depth quality evaluation regions.
[0027] for ,area The depth efficiency is: (9) In the formula, c The subscript indicating the cut point; Indicates the first k The proportion of effective pixels in the candidate region of each shearing point to the total number of pixels; Candidate region for shear point The first 1 pixel; This represents the total number of pixels within the candidate cropping region. M k ( ) indicates the first k Frame pixels p The original depth validity identifier.
[0028] Set depth completion trigger threshold and harvesting safety threshold And satisfy: 1. (10) when When, retain the original effective depth within the candidate region of the shear point; when At that time, RGB-guided depth completion is performed. Specifically, a joint bilateral weighted method is used to estimate the effective depth of the neighborhood within the same target area based on spatial distance and color differences, completing invalid depths while preserving the original effective depths. At this time, no new proximity control command is output. The depth data is reacquired after adjusting the acquisition parameters or the camera observation pose.
[0029] When the depth of the candidate region for the shear point is less than At this time, no new proximity control command is output, and the exposure parameters are adjusted, the camera viewing pose is changed, or the RGB-D image is reacquired based on the current image brightness and depth loss distribution. After obtaining the clipping point, a depth sampling neighborhood is established around the clipping point. Remove zero values, non-finite values, depth values exceeding the effective ranging range, and outliers that deviate from the median of the corresponding neighborhood's initial depth by more than a preset threshold. Use the median of the remaining depth values as the cutoff point depth. (11) In the formula, This represents the set of depth values that meet the criteria of effective ranging range and neighborhood outlier filtering. The depth of the shear point; Indicates the first k After the frame is effectively depth preserved and missing depth is filled, the pixels p The corresponding depth value.
[0030] in, ;(12) In the formula, Represents pixels p The predicted depth of the completion.
[0031] Record the depth efficiency of the shear point and the depth dispersion of its corresponding neighborhood, and use them as the basis for setting the observation noise covariance in motion state estimation. When the depth efficiency decreases or the depth dispersion of the neighborhood increases, increase the noise variance corresponding to the depth observation and decrease the weight of the depth observation in the filter update; obtain the current state estimation covariance after filter update, and then calculate the state prediction covariance at the actual execution time of the control command according to formula (33) to characterize the prediction uncertainty.
[0032] Recognition module: Based on the depth data, the module identifies and associates the fruit and pedicel, and based on the association between the fruit and pedicel, obtains the fruit-pedicel connection point, shearing point and fruit tracking reference point of the current frame.
[0033] An instance segmentation network was used to obtain the passion fruit fruit region and the pedicel region, respectively. The instance segmentation network simultaneously outputs the detection confidence scores for each fruit instance and pedicel instance, denoted as [missing information - likely a scalar value]. and Its value range is .in, Indicates the first Detection confidence of fruit instances in the currently associated combination within the frame. This represents the detection confidence level for the corresponding fruit stalk instance. A higher confidence level indicates a higher degree of certainty from the network regarding the detection result for that instance. Both the fruit and stalk association results are passed to the motion state estimation module to set the noise covariance for the corresponding observations. For the ... The first fruit target and the first b For each fruit stalk target, an association cost is established based on the distance from the stalk tip to the fruit outline, the continuity of the outline, the relative positional relationship, the depth difference, and the predicted position of the target in the previous frame: (13) In the formula, Indicate target With the goal The associated costs between them; Normalized contour distance; The difference in normalization depth; This is the normalized distance between the current detection location and the predicted location in the previous frame. For contour continuity; This ensures the consistency between the current pedicel direction and the historical pedicel direction. to These are the weight coefficients corresponding to each feature. The quantities marked with a horizontal line above all represent feature quantities after normalization.
[0034] The range of values for each normalized cost term is set to... Each weight satisfies: (14) Fruit-stalk combinations with association costs below a set threshold and the lowest cost are selected as the same candidate picking unit. After successful association, a unified target number is assigned to the fruit-stalk combination, so that subsequent cross-frame matching is performed on the complete fruit-stalk combination. Morphological processing, image thinning, and skeleton extraction are performed on the associated stalk region to obtain the stalk centerline. The fruit-stalk connection points are determined based on the connection relationship between the stalk centerline and the fruit outline. And obtain the image domain unit vector pointing from the fruit to the plant. According to the preset stem retention length Depth value corresponding to the fruit-pedicel connection point Based on camera intrinsic parameters and the fruit stalk image orientation, calculate the image domain shear offset adapted to the current target depth, and determine the fruit stalk shear point. : (15) In the formula, This represents the image domain shear offset that varies with the target depth, used to avoid inconsistent actual fruit stalk retention lengths caused by changes in the working distance when using a fixed pixel offset; A unit vector representing the direction of the fruit stalk axis; This represents the equivalent average focal length of the camera, taken as... x , y The average focal length in both directions. The image domain unit vectors representing the fruit-pedicel junction, the shear point, and the direction from the fruit to the plant are respectively represented as: (16) In the formula, , These represent the x and y coordinates of the connection point between the fruit and the pedicel (the base of the pedicel) in the image pixel coordinate system, respectively. , These represent the x and y coordinates of the fruit stem cutting point in the image pixel coordinate system, respectively. , These represent the horizontal and vertical components of the unit vector along the fruit stalk axis in the image pixel coordinate system, respectively.
[0035] in: (17) In the formula, Indicates the first k In the frame image, the square of the magnitude of the unit vector in the direction of the fruit stalk.
[0036] Compared to using a fixed pixel offset, the calculation method employed in this embodiment can reduce the impact of changes in the target working distance on the actual fruit stalk retention length. Shrinkage processing is performed within the fruit segmentation region to obtain candidate tracking regions that avoid the fruit outline and occlusion edges. For candidate pixels... Based on its distance to the fruit outline, local depth quality, segmentation confidence, and distance to the predicted fruit tracking reference point in the previous frame, a fruit tracking reference point score is established: (18) In the formula, G k ( p ) indicates the first k Candidate pixels in frame p As a comprehensive score for the fruit cutting point; The normalized distance from the candidate pixel to the fruit outline; For local depth quality; To segment confidence levels; The normalized distance between the candidate pixel and the predicted clipping point of the previous frame; to These are non-negative weighting coefficients.
[0037] The value range for each rating item is set to... Each weight satisfies: (19) The candidate pixel with the highest overall score and effective depth is selected as the current fruit tracking reference point, denoted as . The reference point is located within the fruit segmentation area and is used to estimate the fruit's motion state; the stem cutting point is determined according to formula (15) and is used to determine the actual cutting position. By establishing the positional relationship between the cutting point and the fruit tracking reference point, a joint estimation of the cutting point's motion state is achieved.
[0038] For multiple passion fruit targets in consecutive image frames, a cross-frame matching cost is established based on target category, predicted pixel location, target region overlap, depth difference, and fruit-stalk association: (20) In the formula, D r Indicates the current detection target and the first r The cost of cross-frame matching between existing target trajectories; The normalized prediction error for the shear point; The normalized depth error of the shear point; This represents the overlap between the current fruit region and the corresponding predicted region. to These are non-negative weighting coefficients.
[0039] Each weight satisfies: .(twenty one) The candidate combination with the lowest matching cost, consistent fruit-stalk association, and matching cost below a set threshold is selected as the current tracking target, thereby reducing the target number switching caused by multiple fruits approaching each other, changes in the order of detection results, or short-term occlusion of the stalk.
[0040] Coordinate recovery module: Based on the fruit-stalk connection point, the fruit tracking reference point, and the shearing point in the current frame, combined with the camera intrinsic parameters, the shearing point depth, and the fixed hand-eye transformation matrix, the module recovers the three-dimensional coordinates of the fruit-stalk connection point, the fruit tracking reference point, and the shearing point in the camera coordinate system and the robot arm base coordinate system; it determines the spatial orientation of the fruit stalk in the robot arm base coordinate system based on the three-dimensional positions of the connection point and the shearing point, and establishes the target geometric constraints required for dynamic approach and clamping / shearing operations.
[0041] For the target pixel coordinates after distortion correction Based on the obtained shear point depth Using camera intrinsic parameters, recover the target point's 3D coordinates in the camera coordinate system: ;(twenty two) In the formula, X C , Y C , Z C These represent the coordinate components of the target point along the X, Y, and Z axes in the camera coordinate system, respectively.
[0042] Connect the fruit and the pedicel respectively and shear point Restored to 3D points in camera coordinate system and Based on the obtained fixed hand-eye transformation matrix and the current end-effector pose of the robotic arm, the 3D points in the camera coordinate system are transformed to the robotic arm base coordinate system: ;(twenty three) In the formula, P B This represents the three-dimensional coordinate vector of the target point in the robot arm's base coordinate system; P C This represents the three-dimensional coordinate vector of the same target point in the camera coordinate system.
[0043] This provides the reference point for fruit tracking in the robotic arm's base coordinate system. Fruit-pedicel junction and shear point The spatial orientation of the fruit stalk in the robot arm's base coordinate system is determined based on the three-dimensional positions of the fruit-stalk connection point and the shearing point. d s,B : .(twenty four) Based on the predicted cutting point, the spatial orientation of the fruit stalk, and the installation offset of the shears, constraints are established for position alignment, cutting direction, opening width, and safety distance. Specifically, the position alignment constraint requires that the image error and depth error of the predicted cutting point relative to the desired cutting position do not exceed preset thresholds; the cutting direction constraint requires that the angle between the normal of the cutting plane and the axis of the fruit stalk does not exceed a preset threshold; the opening width constraint requires that the sum of the width of the fruit stalk cross-section and the positioning error margins on both sides does not exceed the effective opening width of the shears, and that the fruit stalk is located within the effective opening range; and the safety distance constraint requires that the minimum distance between the movement range of the shears during approach and closing and the fruit surface, non-target branches, and the trellis is not less than a preset safety distance.
[0044] The aforementioned target geometric constraints enable the second stage to use the predicted shearing point as the visual servo control target, and to set the desired image position and desired shearing depth based on the installation relationship of the effective shearing center of the scissors relative to the camera. Fruit tracking reference points are used for motion state estimation and shearing point prediction.
[0045] Motion state estimation module: Based on the three-dimensional coordinates and the candidate region depth, an efficient motion state estimation model is established for the fruit tracking reference point and the shearing point; the motion state estimation model includes: state vector, state transition equation, observation vector, and observation equation; through state prediction and visual observation updates, the target image motion velocity, target spatial motion velocity, and state prediction covariance are output; based on the timestamp and feedback from the robotic arm controller, the prediction compensation time is determined, and the shearing point state at the actual execution time of the control command is predicted, thereby obtaining the predicted shearing point.
[0046] To reduce detection fluctuations and compensate for system delays caused by passion fruit swaying, image processing, control calculations, CAN communication, and robotic arm response, motion state estimation models were established for the fruit tracking reference point and shearing point, respectively.
[0047] Define the relative state vector of the shearing point with respect to the fruit tracking reference point as follows: (25) In the formula, Indicates the first k The relative state vector of the cut-out point in the frame with respect to the fruit tracking reference point; This represents the difference between the state variables corresponding to the shearing point and the fruit tracking reference point; These represent the differences in horizontal pixel coordinates, vertical pixel coordinates, and depth between the cut-off point and the fruit tracking reference point, respectively, all calculated by subtracting the fruit tracking reference point from the cut-off point. and These represent the horizontal and vertical pixel coordinates of the cut-off point and the fruit tracking reference point, respectively. , These represent the depths of the shearing point and the fruit tracking reference point along the optical axis in the camera coordinate system, respectively.
[0048] By combining the absolute state of the fruit tracking reference point, the relative state of the shearing point with respect to the fruit tracking reference point, and their respective rates of change, a joint state vector is established: (26) In the formula, superscript T The dot (.) indicates transpose; the superscript dot (.) indicates the first derivative of the variable with respect to time.
[0049] This joint state vector representation does not predict two unrelated image points independently, but instead utilizes the geometric relationship that the fruit and the corresponding pedicel remain connected in a short time to obtain the state of the pedicel shear point by predicting the shear point.
[0050] State prediction is performed using a discrete uniform motion model: (27) In the formula, , These represent the state at the next time step and the state at the current time step, respectively. Represents the state transition matrix; This indicates process noise.
[0051] State transition matrix It consists of two sub-matrices corresponding to the relative states at the shear point, arranged in a block diagonal form. Both states adopt a discrete uniform motion model, and their state transition sub-matrices have the same form, which are uniformly denoted as... ,Right now: (28) In the formula, This represents the actual time interval between two consecutive visual detections.
[0052] The visual observation vector consists of the cutoff point position, the cutoff point depth, and the position and depth difference between the cutoff point and the cutoff point: (29) In the formula, Indicates the first k The visual observation vector of a frame; superscript obs This represents the observations actually detected by the vision system; To observe noise; H This represents the observation matrix.
[0053] According to the order of the joint state vectors in formula (26), the observation matrix H Represented as: (30) In the formula, It is a third-order identity matrix. It is a third-order zero matrix.
[0054] Observation matrix The shearing point position state is extracted from the joint state vector. The main control module then uses the fruit detection confidence score output by the recognition module. Confidence level of fruit stalk detection The fruit-stalk association cost, as well as the depth efficiency of the regions corresponding to the fruit tracking reference point and the shearing point, are used to set the observation noise covariance. When the fruit detection confidence decreases, the noise variance of the observation corresponding to the fruit tracking reference point is increased; when the stalk detection confidence decreases or the fruit-stalk association cost increases, the noise variance of the observation corresponding to the relative state of the shearing point is increased, thereby reducing the weight of the corresponding observation in the filter update. When the stalk cannot be reliably detected temporarily but the fruit can still be reliably detected, the absolute state of the fruit tracking reference point is updated using the effective observations, and the shearing point prediction is maintained by combining the existing relative state within the allowable range of consecutive lost frames and prediction covariance.
[0055] After state prediction and visual observation updates, the updated state estimate is taken as the current state. To simplify subsequent expressions, the position, depth, and rate of change in the state vector represent filtered and updated estimates, rather than the unprocessed raw observations. Based on the timestamps of each module and feedback from the robotic arm controller, the prediction compensation time between the current image acquisition time and the actual execution time of the control command is determined. τ : (31) In the formula, For image acquisition and transmission delay; Delay in target detection; Due to the latency of deep processing; To control computational latency; For CAN communication delay; This is due to the response delay of the robotic arm.
[0056] Each delay component is updated online based on the image timestamp, algorithm start and end timestamps, CAN message sending and responding timestamps, and the actual response time of the robotic arm, instead of using only a fixed next frame time as the prediction time.
[0057] Predict the relative state of the cutoff point at the actual execution time of the control command, and thereby recover the predicted cutoff point: (32) In the formula, These represent the x-coordinate, y-coordinate, and depth of the predicted shear point image, respectively. This represents the current state observation value of the shear point. This represents the rate of change corresponding to the observed state value at the shear point at the current moment; This refers to the delay in visual processing and control command execution, i.e., the actual prediction compensation time.
[0058] The prediction covariance at the actual execution time of the control command is: (33) In the formula, To compensate for the actual predicted time Construct the state transition matrix; This indicates the covariance estimate of the current state, to avoid interference with points in three-dimensional space. and The symbols are confused; This is the process noise covariance over the prediction time range.
[0059] When the depth efficiency of the connection point or shearing point is lower than the operational safety threshold, the fruit-stalk association cost exceeds the set threshold, or the prediction covariance exceeds the set upper limit, the system prohibits the gripping and shearing actions and controls the robotic arm to maintain its current position or return to a safe observation pose. When the stalk is briefly obscured or missed, the shearing point prediction is maintained based on the relative state of the shearing point within the maximum allowable consecutive frame loss and relative state covariance threshold range. When the consecutive frame loss or prediction uncertainty exceeds the set threshold, the current fruit-stalk combination number is cleared, and target detection, pairwise association, and state initialization are re-executed.
[0060] After the first stage is completed, the main control module outputs the predicted shear point. The depth efficiency and state prediction covariance are calculated. In the second stage, the IBVS image error is established based on the above prediction results, and the end effector of the robotic arm is controlled to continuously align, dynamically approach, clamp, and cut the passion fruit stem in motion.
[0061] Target pose update module: Converts the predicted shearing point into normalized image coordinates and establishes the predicted image feature error; extracts the rotation control sub-matrix based on the image interaction matrix of the target point, and then combines the predicted image feature error to obtain the camera angular velocity control quantity; when the image error enters the set range, generates the approach velocity in the direction of the camera optical axis based on the error between the predicted shearing depth and the expected shearing depth in the predicted shearing point; obtains the visual servo velocity vector in the camera coordinate system from the camera angular velocity control quantity and the approach velocity in the direction of the camera optical axis, and then obtains the new target pose of the camera and the new target pose of the robotic arm end effector.
[0062] Convert the predicted pixel coordinates to normalized image coordinates: (34) In the formula, x , y These represent the x-coordinate and y-coordinate of the predicted target point in the normalized image coordinate system, respectively.
[0063] Based on the installation relationship between the camera and the end effector, the desired shearing feature position is determined in advance. x d ,y d and desired shear depth Z d For cases where the camera's optical axis coincides with the shearing center of the end effector, an adjustment can be made. x d =0、 y d =0.
[0064] Predicting image feature error e for: (35) Image interaction matrix of target point L s for: (36) Based on the robotic arm tracking method, from the image interaction matrix L s Extracting the corresponding camera orbit x shaft and y The 4th and 5th columns of the axis rotation yield the rotation control submatrix. L R : (37) The camera angular velocity control value is: (38) In the formula, for L R Moore-Penrose generalized inverse matrix; λ IBVS control gain; , These represent the camera's coordinates around its own coordinate system. x shaft and y Angular velocity control component of the shaft.
[0065] Set adaptive control gain based on the magnitude of image error: ; (40) In the formula, λ min and λ max These are the minimum and maximum values of the IBVS control gain, respectively. escale This is the normalized scale parameter for image error.
[0066] When the image error is large, a larger control gain is used to improve the target tracking speed; when the image error is small, a smaller control gain is used to reduce end-effector jitter. Once the target image error falls within a set range, the approach speed along the camera's optical axis is generated based on the error between the predicted target depth and the desired shear depth. v z : (41) When the depth error is less than the depth dead zone, let v z =0; When the image error has not yet entered the allowable range, limit or stop the approach movement in the optical axis direction, allowing the robotic arm to first complete target alignment and then gradually approach the target. The visual servo velocity vector in the camera coordinate system is composed of angular velocity and optical axis velocity: (42) In the formula, ξ C The Lie algebra matrix corresponding to the velocity vector; This represents the angular velocity control component of the camera about its own coordinate system's z-axis; , These represent the camera's coordinates along its own coordinate system. x shaft and y The linear velocity control component of the shaft.
[0067] According to the control cycle Δ t Calculate the incremental homogeneous transformation matrix in the camera coordinate system: (43) In the formula, This represents the homogeneous transformation matrix of pose increment in the current camera coordinate system within a control cycle.
[0068] in, (44) According to formula (45) v x =0、 v y =0、 ω z =0, which can be further written as: (45) New target pose of the camera for: (46) In the formula, This represents the pose homogeneous transformation matrix of the current camera coordinate system relative to the robot arm's base coordinate system.
[0069] and the new target pose at the end of the robotic arm for: (47) In the formula, This represents the homogeneous pose transformation matrix of the current end-effector coordinate system relative to the robot arm base coordinate system.
[0070] The main control module will The position and attitude parameters are converted into those used by the robotic arm controller and tracking commands are periodically sent via the CAN bus in a non-blocking manner. Limits are imposed on the maximum rotational increment, maximum axial displacement, maximum joint velocity, and maximum joint acceleration within each control cycle to prevent fluctuations in target detection from causing the robotic arm to momentarily reverse or exhibit large jumps.
[0071] Dynamic shearing pose estimation module: Based on the predicted shearing point coordinates and predicted depth, calculates the target's three-dimensional position at the moment the shears close and transforms it to the robot arm's base coordinate system; constructs the dynamic shearing target pose based on the predicted shearing point, fruit stem direction, and end effector installation offset; when the predicted image feature error, depth error, target motion speed, depth efficiency, and robot arm status simultaneously meet the set conditions, and the set conditions are continuously maintained for a set number of frames, the fruit-fruit stem correlation remains consistent, the predicted shearing point is located within the robot arm's allowed working space, and the robot arm has no collision risk or malfunction, the shearing operation is allowed; otherwise, returns to the target pose update module, or stops approaching and returns to the pre-shearing safe pose.
[0072] Based on the predicted pixel coordinates and predicted depth obtained from Kalman filtering, the target's 3D position at the moment of shear cutting is calculated. Let the time required for the shears to complete clamping from receiving the closing command be... τ g Then the predicted target state at the closing time is: (48) The predicted position of the target in the camera coordinate system at the moment of closure is obtained based on the camera intrinsic parameters: (49) In the formula, This indicates the predicted position in the camera coordinate system during the shear cut; , , These represent the predicted coordinate components of the clipping point on the three coordinate axes of the camera coordinate system.
[0073] Transform it to the robot arm's base coordinate system: (50) In the formula, , , These represent the predicted coordinate components of the shearing point on the three coordinate axes of the robot arm's base coordinate system.
[0074] The dynamic shearing target pose is constructed based on the predicted shearing point, stalk direction, and end effector installation offset. (51) In the formula, This represents the homogeneous transformation matrix of the target pose relative to the robot arm's base coordinate system when the robot arm's end effector performs a shearing operation. for The target rotation matrix represents the desired pose of the robotic arm's end effector relative to the base coordinate system; for The target position vector represents the desired position of the origin of the robot arm's end effector coordinate system in the base coordinate system.
[0075] Before cutting, the shear opening width constraint and the cutting safety distance constraint are checked based on the dynamic cutting target pose. The shear opening width constraint means that, under the predetermined cutting pose, the sum of the width of the target fruit stalk within the cutting section and the positioning error margins on both sides does not exceed the effective opening width of the shears, and the target fruit stalk is located within the effective opening range. The cutting safety distance constraint means that the range of motion of the shears during approach and closing processes maintains a preset safe distance from the fruit surface, non-target branches, and supports. If all the above constraints are met, subsequent cutting condition judgments continue; if any constraint is not met, cutting is not performed, and the target pose is readjusted.
[0076] The shearing operation is allowed when the target pixel error, depth error, target motion speed, depth efficiency, and robotic arm status all meet the following conditions: (52) In the formula, The allowable threshold representing the L2 norm of the normalized image feature error; Indicates the allowable threshold for depth error; This indicates the velocity of the fruit target in the robot arm's base coordinate system; This indicates the maximum allowable tracking speed at the end effector of the robotic arm.
[0077] At the same time, the above conditions must be maintained continuously. N s The frame is consistent, the fruit-stalk relationship remains unchanged, the predicted shear point is within the robot arm's allowable workspace, and the robot arm has no collision risk or malfunction. If the above conditions are not met, the robot arm continues to perform IBVS dynamic tracking; if the target speed exceeds the robot arm's allowable tracking speed, the target is lost for a long time, or the predicted shear point is outside the workspace, the approach stops and the robot arm returns to the pre-shear safe pose.
[0078] Dynamic cutting execution module: Establishes a kinematic model of the robotic arm based on the dynamic cutting target pose; solves the joint variables of the robotic arm based on the dynamic cutting target pose; when the cutting conditions are met for a set number of consecutive frames, controls the end effector to close and cuts off the fruit stem, transferring the fruit to the placement area.
[0079] A kinematic model of the robotic arm is established based on the pose of the dynamically sheared target. For the first... i For each joint, the standard DH homogeneous transformation matrix between the coordinate systems of adjacent joints is: (53) In the formula, , They represent the first Joint angle The cosine and sine values; , They represent the first Linkage torsion angle The cosine and sine values; Indicates the first The length of each link; Indicates the first The torsion angle of each link.
[0080] The positive kinematics of the robotic arm's end effector relative to the base coordinate system are: (54) In the formula, This represents the homogeneous transformation matrix of the robotic arm's end effector relative to the base coordinate system; This represents the positive kinematic mapping function of the robotic arm.
[0081] Solve the joint variables of the robotic arm based on the dynamic shearing target pose. q : (55) In the formula, p ( q )and R ( q These are the end-effector position vector and attitude rotation matrix corresponding to the current joint variable, respectively; p d and R d These are the desired end-effector position vector and the desired orientation rotation matrix corresponding to the pose of the dynamically sheared target, respectively. w p and w R These are the weighting coefficients for position error and attitude error, respectively.
[0082] The current end position vector and the desired end position vector are respectively: (56) In the formula, , , These represent the current mechanical joint variables. The corresponding position vector of the robotic arm end effector in the base coordinate system Three components; , , These represent the three components of the end position vector corresponding to the desired shearing target pose in the base coordinate system.
[0083] The current end-effector attitude rotation matrix is: (57) In the formula, r ij ( q ) represents the current mechanical joint variable Corresponding end attitude rotation matrix The Middle Line 1 The elements of the column.
[0084] The desired end-effector rotation matrix is: (58) In the formula, r dij This represents the attitude rotation matrix corresponding to the desired shear target pose. The Middle Line 1 The elements of the column.
[0085] During the dynamic tracking phase, the robotic arm executes the following sequence: target detection, state prediction, IBVS tracking, shearing condition determination, end effector closure, withdrawal, and placement. Once the shearing conditions are met for a set number of consecutive frames, the control system sends a closure command to the end effector. During shear closure, the main control module continues to acquire images and perform IBVS dynamic tracking, ensuring the shears continuously follow the target during the shearing process, compensating for positional changes caused by fruit swaying, chassis vibration, and control delays. When the end effector detects that the clamping current, clamping position, or clamping force has reached a set threshold, it determines that the fruit has been reliably clamped, and then controls the shearing mechanism to close and cut the fruit stem. After shearing, the robotic arm moves along the predetermined withdrawal direction to a safe position and then transfers the fruit to the placement area.
[0086] Loop tracking module: When a target is lost, movement exceeds limits, communication is abnormal, or actuator malfunctions, the control robot arm stops its current action, returns to a safe position, or re-enters the target detection and tracking process. After the current target is successfully picked, the target status is updated and the picking process for the next target to be picked begins.
[0087] The control system encapsulates the end-effector pose, joint target position, running speed, and operating mode obtained from IBVS dynamic tracking into CAN bus control messages and sends them to the joint drivers of the robotic arm. The control system receives joint position, joint speed, drive current, end-effector position, gripping status, and fault status in real time and sends the feedback results to the main control module. The main control module dynamically adjusts the control input for the next control cycle based on the error between the actual state of the robotic arm and the target state. When target loss, abnormal depth, joint over-limit, collision risk, gripping failure, shearing failure, or CAN communication failure occurs, the system stops the current approach or shearing action, returns the robotic arm to a safe pose, and re-performs target detection and status assessment. After harvesting, the robotic arm leaves the work area and returns to the observation pose. The main control module updates the target status and enters the next target detection, dynamic tracking, and harvesting cycle.
[0088] Example 2: To verify the target perception accuracy, dynamic tracking capability, and actual harvesting effect of the proposed solution, an automated passion fruit harvesting robot test platform was constructed. The test platform consists of a mobile chassis, a robotic arm, an RGB-D vision acquisition module, an end effector for gripping and shearing, a main control computing unit, and a CAN communication control system. The vision acquisition module is fixed to the end of the robotic arm using an "eye-on-hand" mounting method, used to continuously acquire RGB images and depth information of the passion fruit and its stem. Experiments were conducted in actual passion fruit growing environments or simulated orchard environments. By setting passion fruit targets with different spatial positions and swing states, the target localization, motion prediction, dynamic visual tracking, and harvesting execution effects of the proposed method were tested. Figure 2 Experiments were conducted in a medium-environment environment.
[0089] In the first phase, the performance of target perception, 3D localization, and motion state prediction was primarily verified. Passion fruit targets at different distances, spatial positions, and degrees of occlusion were selected. RGB and depth images were continuously acquired through the vision acquisition module, and the detection results of the fruit, stem, and shearing position were recorded. Using the target's spatial position obtained through manual measurement or calibration as a reference value, the 3D coordinates recovered by the vision system were compared with the reference coordinates. The localization errors in the X, Y, and Z directions and the overall 3D localization error were statistically analyzed. Simultaneously, for passion fruit targets in a swinging state, their actual movement position and the system's predicted position were continuously recorded. The target's movement trajectory and the predicted trajectory were compared, and the average prediction error under different movement states was statistically analyzed. The experimental results show that the system can maintain stable recognition of passion fruit and stems. The average error of the overall 3D localization of the target is 1.7 mm, and the average error of the target motion prediction is 1.5 mm, providing reliable target state information for subsequent dynamic tracking and shearing position determination by the robotic arm. Localization information is as follows: Figure 3 As shown.
[0090] In the second phase, the performance of IBVS dynamic visual tracking and closed-loop harvesting was verified. During the experiment, passion fruit was kept stationary and subjected to varying degrees of swaying. The robot first determined the target to be harvested based on visual perception, and then continuously tracked the target using the IBVS-controlled end effector. The lateral and longitudinal visual errors between the target's current image position and the desired position were recorded, and the changes in visual error over time during the robot's approach to the target were analyzed. Once the target gradually entered the effective working area of the end effector, the system determined the shearing conditions based on the target's visual deviation, spatial distance, relative motion state, and shearing point position, and controlled the robot and end effector via the CAN bus to complete the approach, gripping, and shearing actions. The experimental results showed that the robot could continuously correct the end effector's trajectory based on real-time visual feedback when the target swayed, and the visual error gradually converged to the preset range, reaching a stable tracking state within 1.71 seconds. After 37 dynamic harvesting experiments, 34 harvests were successfully completed, achieving a dynamic harvesting success rate of 92%, with an average harvesting time of 3.72 seconds per fruit. (Details are as follows...) Figure 4 As shown.
[0091] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made to the technical solutions of the present invention by those skilled in the art without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.
Claims
1. An automatic passion fruit identification and harvesting system, characterized in that, include: Calibration and initialization module: calibrates the camera intrinsic parameters, distortion parameters, and fixed hand-eye transformation matrix, and initializes the camera intrinsic parameters, distortion parameters, fixed hand-eye transformation matrix, robotic arm joint limits of the picking robot, end effector installation offset, effective range of camera, and control cycle; Image acquisition and processing module: The vision acquisition module of the harvesting robot is used to acquire RGB images and raw depth maps of passion fruit and stems in the working area; The depth quality is determined based on the RGB image and the original depth map to obtain depth data; the depth data includes: effective depth mask, candidate region depth effectiveness rate, completed depth map, and clipping point depth; Recognition module: Based on the depth data, the module identifies and associates the fruit and pedicel, and based on the association between the fruit and pedicel, obtains the fruit-pedicel connection point, shearing point and fruit tracking reference point of the current frame. Coordinate recovery module: Based on the fruit-stalk connection point, the fruit tracking reference point, and the shearing point in the current frame, combined with the camera intrinsic parameters, the shearing point depth, and the fixed hand-eye transformation matrix, the module recovers the three-dimensional coordinates of the fruit-stalk connection point, the fruit tracking reference point, and the shearing point in the camera coordinate system and the robot arm base coordinate system; it determines the spatial orientation of the fruit stalk in the robot arm base coordinate system based on the three-dimensional positions of the connection point and the shearing point, and establishes the target geometric constraints required for dynamic approach and clamping / shearing operations; Motion state estimation module: Based on the three-dimensional coordinates and the candidate region depth, it efficiently establishes motion state estimation models for the fruit tracking reference point and the shearing point; the motion state estimation model includes: state vector, state transition equation, observation vector, and observation equation; through state prediction and visual observation updates, it outputs the target image motion speed, target spatial motion speed, and state prediction covariance; based on the timestamp and feedback from the robotic arm controller, it determines the prediction compensation time and predicts the shearing point state at the actual execution time of the control command, thereby obtaining the predicted shearing point; Target pose update module: Converts the predicted shear point into normalized image coordinates and establishes the predicted image feature error; extracts the rotation control sub-matrix based on the image interaction matrix of the target point, and then combines the predicted image feature error to obtain the camera angular velocity control quantity; when the image error enters the set range, it generates the approach velocity in the direction of the camera optical axis based on the error between the predicted shear point depth and the desired shear depth; obtains the visual servo velocity vector in the camera coordinate system from the camera angular velocity control quantity and the approach velocity in the direction of the camera optical axis, and then obtains the new target pose of the camera and the new target pose of the robotic arm end effector. Dynamic shearing pose estimation module: Based on the predicted shearing point coordinates and predicted depth, calculates the target's three-dimensional position at the moment of shear closure and transforms it to the robot arm's base coordinate system; constructs the dynamic shearing target pose based on the predicted shearing point, fruit stem direction, and end effector installation offset; when the predicted image feature error, depth error, target motion speed, depth efficiency, and robot arm status simultaneously meet the set conditions, and the set conditions are continuously maintained for a set number of frames, the fruit-fruit stem correlation remains consistent, the predicted shearing point is located within the robot arm's allowed working space, and the robot arm has no collision risk or malfunction, dynamic shearing is allowed; otherwise, returns to the target pose update module, or stops approaching and returns to the pre-shearing safe pose; Dynamic cutting execution module: Establishes a kinematic model of the robotic arm based on the dynamic cutting target pose, and then solves the joint variables of the robotic arm; when the cutting conditions are met continuously for a set number of frames, controls the end shears to close and cut the fruit stem, transferring the fruit to the placement area; Loop tracking module: When a target is lost, movement exceeds limits, communication is abnormal, or actuator malfunctions, the control robot arm stops its current action, returns to a safe position, or re-enters the target detection and tracking process. After the current target is successfully picked, the target status is updated and the picking process for the next target to be picked begins.
2. The passion fruit automatic identification and harvesting system according to claim 1, characterized in that, The fixed hand-eye transformation matrix includes: ; In the formula, R EC This is the rotation matrix from the camera coordinate system to the robot arm end effector coordinate system; t EC is the position vector of the origin of the camera coordinate system in the coordinate system of the end effector of the robotic arm; r ij For the camera coordinate system j The coordinate axis is related to the coordinate system of the robotic arm's end effector. i The cosine of the angle between the coordinate axes; t x , t y , t z These represent the positions of the robotic arm's end effector along... x , y , z Components of axis translation This is a fixed hand-eye transformation matrix from the camera coordinate system to the end effector coordinate system of the robotic arm.
3. The passion fruit automatic identification and harvesting system according to claim 1, characterized in that, for ,area The depth efficiency is: ; In the formula, c The subscript indicating the cut point; Indicates the first k The proportion of effective pixels in the candidate region of each shearing point to the total number of pixels; Candidate region for shear point The first 1 pixel; This represents the total number of pixels within the candidate cropping region. M k ( ) indicates the first k Frame pixels p The original depth validity identifier; The shear point depth includes: ; In the formula, This represents the set of depth values that meet the criteria of effective ranging range and neighborhood outlier filtering. The depth of the shear point; Indicates the first k After the frame is effectively depth preserved and missing depth is filled, the pixels p The corresponding depth value.
4. The passion fruit automatic identification and harvesting system according to claim 1, characterized in that, The method for obtaining the predicted shear point includes: ; In the formula, These represent the x-coordinate, y-coordinate, and depth of the predicted shear point image, respectively. This represents the current state observation value of the shear point. This represents the rate of change corresponding to the observed state value at the shear point at the current moment; This refers to the latency of visual processing and control command execution.
5. The passion fruit automatic identification and harvesting system according to claim 1, characterized in that, New target pose of the camera for: ; In the formula, This represents the pose homogeneous transformation matrix of the current camera coordinate system relative to the robot arm's base coordinate system; This represents the pose increment homogeneous transformation matrix expressed in the current camera coordinate system within a control cycle; and the new target pose at the end of the robotic arm for: ; In the formula, This represents the homogeneous pose transformation matrix of the current end-effector coordinate system relative to the robot arm base coordinate system.
