A method for kiwifruit picking peduncle posture estimation, pre-classification and grasping pose solving

Abstract

The application discloses a method for kiwi fruit picking, fruit axis posture estimation, pre-classification and grasping pose solving, comprising the following steps: step S1, fruit rough positioning and rough classification, step S2, fruit axis key point automatic positioning and fruit axis direction solving, step S3, picking order planning based on space clustering and category depth priority, step S4, fruit equator diameter pre-classification calculation, and step S5, grasping pose solving and execution. The application can complete fruit posture recognition, picking order planning, size classification and optimal grasping pose solving simultaneously at the picking moment without additional key point labeling, thereby effectively solving the problems of picking and classification separation, lack of geometric reference for grasping direction and key point dependence on artificial labeling in the prior art.

Description

Technical Field

[0001] This invention relates to the field of agricultural robot technology, specifically to a method for kiwi fruit axis pose estimation, equatorial diameter pre-grading, and grasping pose solution based on RGB-D vision. Background Technology

[0002] In mechanized kiwifruit harvesting, online perception of fruit pose is a prerequisite for non-destructive grasping and pre-grading at the picking end. The fruit axis direction (the direction of the line connecting the calyx to the fruit apex) is the most important geometric reference of the fruit, directly affecting the planning of the gripper's feeding posture and the success rate of grasping. Currently, the localization of key points of the fruit mainly relies on heat map regression methods based on deep learning (such as YOLO-Pose and HRNet). These methods require manual annotation of the calyx and fruit apex positions on a large number of images, which is costly and time-consuming, and the generalization ability of the model is limited in different varieties or growing environments. In addition, existing harvesting robot vision systems mostly use detection box center or point cloud principal component analysis to approximate the fruit pose, lacking explicit localization of key points with clear biological semantics such as the calyx and fruit apex. This leads to a deviation between the grasping direction and the actual main axis of the fruit, which can easily cause problems such as gripper skew, fruit slippage, or fruit stem tearing. In fruit size measurement, existing methods typically approximate the fruit diameter by using the width or height of the detection frame, failing to fully utilize the fine contour information provided by instance segmentation masks. This makes it difficult to accurately obtain the equatorial diameter for pre-grading at the picking end and adaptive control of the gripper opening. Therefore, there is an urgent need for a method that can automatically locate the semantic key points of the calyx and apex without additional keypoint annotation, and calculate the fruit axis direction and equatorial size accordingly, to meet the requirements of kiwifruit harvesting robots for real-time performance, robustness, and non-destructive operation. Summary of the Invention

[0003] This invention provides a method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting. This method does not require additional key point annotation and can simultaneously complete fruit posture recognition, harvesting sequence planning, size grading, and optimal grasping pose solution at the moment of harvesting. This effectively solves the problems of separation of harvesting and grading, lack of geometric reference for grasping direction, and reliance on manual annotation of key points in the prior art.

[0004] The present invention adopts the following technical solution:

[0005] A method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting is characterized by the following steps:

[0006] Step S1, coarse fruit localization and coarse classification:

[0007] Based on the YOLO target detection network, the fruit is quickly and coarsely located and classified according to its occlusion status. The target is classified according to the spatial occlusion status of the fruit, specifically into two categories: unoccupied independent fruit (KA) and occluded fruit (KF) blocked by leaves, branches, or other fruits. This provides a priority determination basis for multi-target picking sequence planning.

[0008] Step S2, automatic positioning of key points on the fruit axis and calculation of the fruit axis direction:

[0009] The binary mask of the fruit is obtained through an instance segmentation model. The binary mask is preprocessed and its contour is extracted. Then, an ellipse is fitted. Based on the endpoints of the major axis of the ellipse and combined with the prior knowledge of the fruit's natural growth posture, the calyx point is automatically located. With the apex The two-dimensional pixel coordinates; combined with the depth map and camera intrinsic parameters, the... , Transform pixel coordinates to camera coordinates and calculate the unit direction vector of the fruit axis. ;

[0010] Step S3, picking order planning based on spatial clustering and category depth priority:

[0011] Spatial clustering is performed based on the fruit center coordinates to divide fruits that are physically close into the same picking cluster. For the fruits within each cluster, the picking order within the cluster is generated according to the rule of "independent fruits first, and fruits of the same type from near to far in depth".

[0012] Step S4, Pre-grading and calculation of fruit equatorial diameter:

[0013] Define the center of the equator Midpoint of the fruit axis With the geometric center of the mask Orthogonal projection point on the fruit axis line Weighted fusion, by preserving actual observation information while suppressing center offset caused by attitude tilt or segmentation errors, improves the robustness of the measurement. Using the equatorial center as a reference, the mask boundary is sampled along the fruit axis normal vector direction to obtain the two equatorial endpoints, and the projected diameter is calculated respectively. With three-dimensional space diameter The final equatorial diameter is obtained by weighted fusion of the two. ,Will As a basis for pre-grading of kiwifruit fruit diameter and picking end, it is used to control the opening of the gripper and realize the coarse sorting of fruits by size.

[0014] Step S5, Capture Pose Solving and Execution:

[0015] Centered on the equator To capture the target position, along the fruit axis direction For the gripper feed direction, a set of candidate gripping poses is generated by enumerating candidate roll angles. Then, the optimal gripping pose is selected by solving inverse kinematics and evaluating the comprehensive cost function. The pose is then transformed to the base coordinate system of the collaborative robotic arm via a coordinate transformation link, and the robotic arm is controlled to perform the picking action.

[0016] Furthermore, in step S2 of this invention, the preprocessing process involves using the trained YOLO11x-seg model to infer the input RGB image, obtaining the original binary segmentation mask of the kiwi fruit, smoothing the mask using Gaussian blur to eliminate jagged edges and holes, then binarizing it using a fixed threshold and extracting all external contours from the binary image, selecting the connected component with the largest area, thereby obtaining a mask image with complete shape, smooth edges, and removed minor noise.

[0017] Furthermore, the ellipse fitting in step S2 of this invention adopts Fitzgibbon ellipse fitting, that is, the direct least squares ellipse fitting algorithm is used to fit the contour point set to an ellipse, and obtain the geometric parameters of the ellipse, namely the center coordinates, the length of the semi-axis and the rotation angle. The consistency between the major semi-axis and the rotation angle is ensured by the correction logic. The correction logic is as follows: if the minor semi-axis is greater than the major semi-axis, the major and minor semi-axis are swapped and the rotation angle is increased by 90°, and the increased rotation angle is processed by modulus 180°.

[0018] Furthermore, the fruit vertex described in step S2 of the present invention The rule for determining the two-dimensional pixel coordinates is as follows: compare the ordinates of the two endpoints of the major axis of the ellipse, and mark the endpoint with the larger ordinate as the calyx point. The endpoint with the smaller ordinate is marked as the fruit vertex. The discrimination rule for the larger endpoint of the ordinate, representing the calyx point M, relies on the biological priori knowledge that kiwifruit grows in a natural hanging posture with the calyx end facing down and the fruit apex facing up in the target harvesting area. Before harvesting, the system uses hand-eye calibration and camera pose constraints to ensure that the image Y-axis direction is approximately parallel to the direction of gravity, thus ensuring that the relative sizes of the ordinates stably correspond to the semantics of the calyx and fruit apex. Combining depth maps and camera intrinsic parameters, the system... , The two-dimensional pixel coordinates are transformed to the camera coordinate system to obtain the three-dimensional spatial coordinates of both. , Then calculate the unit direction vector of the fruit axis. , direction from point to In this process, the depth map and the RGB map are aligned temporally to establish a pixel-level correspondence, ensuring that key points of the same fruit are aligned. , The pixel coordinates can obtain the source depth value.

[0019] Furthermore, the spatial clustering described in step S3 of this invention employs the K-Means clustering algorithm, utilizing the position of the fruit center in the camera coordinate system. , K-Means clustering is performed on the coordinates to divide nearby fruits in the physical space into the same picking cluster; for fruits within each cluster, a picking sequence is generated according to the rule of "independent fruits first, and fruits of the same type from near to far in depth"; for different clusters, the clusters containing independent fruits and closer to the camera are traversed first.

[0020] Furthermore, in step S4 of the present invention, the equatorial center is defined. Midpoint of the fruit axis With the geometric center of the mask Orthogonal projection point on the fruit axis line The weighted fusion, its The calculation formula is ,

[0021] in, From the calyx point With the apex The midpoint is determined, representing the geometric prior center of the fruit along the fruit axis. For the geometric center of the mask Orthogonal projection onto the fruit axis eliminates observation offsets perpendicular to the fruit axis. These are the preset weighting coefficients.

[0022] In step S4, using the center of the equator as a reference, the vector is extended to both sides along the direction of the fruit axis normal vector to the mask boundary, thus obtaining the two endpoints of the equator. , Calculate the pixel span from the pixel coordinates. Combined with the camera's equivalent focal length Reference depth with the center of the equator Calculate the physical diameter using the principle of similar triangles.

[0023] This is the equivalent focal length of the camera.

[0024] Furthermore, the three-dimensional spatial diameter mentioned in step S4 of the present invention The 3D Euclidean distance between the two points is calculated by transforming the pixel coordinates of the two equatorial endpoints to the camera coordinate system. The method involves... , Transform to the camera coordinate system and calculate the true Euclidean distance between the two in 3D space. ;Will and The final equatorial diameter is obtained by weighted fusion. It is used as the basis for judging the diameter of kiwifruit (the basis for judging fruit size) and the basis for pre-grading at the picking end, so as to control the opening of the gripper and realize the coarse sorting of fruits by size.

[0025] Furthermore, the candidate roll angle mentioned in step S5 of the present invention is within the range of [-180°, 180°] with a fixed step size. Enumerate to generate a set of candidate grab poses.

[0026] Furthermore, the comprehensive cost function described in step S5 of the present invention includes at least one or more of the following four items: motion amplitude cost, joint limitation cost, motion singularity cost, and posture preference cost.

[0027] Furthermore, the coordinate transformation link in step S5 of the present invention includes sequential transformations between the kiwi coordinate system {M}, the camera coordinate system {C}, the flange coordinate system {T}, the gripper tool coordinate system {G}, and the collaborative robotic arm base coordinate system {B}.

[0028] This invention offers the following advantages: By employing the above technical solutions, this invention provides an integrated method that eliminates the need for additional keypoint annotation, enabling simultaneous fruit posture recognition, harvesting sequence planning, size grading, and optimal grasping pose calculation during harvesting. This addresses issues in existing technologies such as separation of harvesting and grading, lack of geometric reference for grasping direction, and reliance on manual annotation of keypoints. During implementation, this invention requires no additional keypoint annotation data; it automatically locates the calyx and apex points solely based on instance segmentation masks, significantly reducing data preparation costs. Spatial clustering and depth-first harvesting sequence planning reduce robotic arm motion redundancy and improve harvesting efficiency. Using the fruit axis as the feed direction, non-destructive grasping is achieved through candidate posture enumeration and cost function optimization, avoiding fruit damage. This invention obtains the major axis endpoints through ellipse fitting of instance segmentation masks and, combined with biological priors regarding the natural hanging posture of kiwifruit and camera pose constraints, semantically represents the endpoints as calyx points. With the apex This invention establishes a fruit axis direction vector with clear physical meaning, fundamentally solving the problem of misalignment between the gripping direction and the fruit's main axis. First, the equatorial center is determined by weighted fusion of the fruit axis midpoint and the projection point of the mask centroid. Then, the equatorial endpoints are obtained by sampling along the fruit axis normal on the mask boundary. The projected diameter in the image plane and the true Euclidean distance in three-dimensional space are calculated respectively. After cross-validation and weighted fusion, the final equatorial diameter is output. Weighted fusion of the equatorial center and cross-validation of the projected / three-dimensional diameter achieve high-precision diameter measurement, for example, an average absolute error of 0.85mm. It supports pre-grading at the sampling end. This invention uses "TCP coinciding with the fruit's equatorial center point, and the gripper feed axis aligning with the fruit axis direction..." Collinearity is a hard constraint, reducing the degrees of freedom of the grasping pose to a single roll angle around the feed axis. The optimal grasping pose is output by enumerating candidate poses, solving inverse kinematics, and optimizing a comprehensive cost function (motion amplitude, joint limits, singularity, and posture preference). This method ensures non-destructive grasping while balancing the robot arm's motion efficiency and safety. This invention simultaneously calculates the equatorial diameter and pre-grades the fruit at the picking point, using the dimensional information for both gripper opening control and coarse fruit sorting, achieving integrated "picking and sorting" operations. This invention reduces secondary handling steps, effectively protecting the appearance and quality of the fruit. Based on a single visual perception chain (fruit axis orientation, equatorial diameter calculation) and a high-precision positioning platform, it performs harvesting and pre-grading operations during the harvest season. During the off-season, it can switch to a growth inspection mode, repeatedly scanning the same fruit cluster along a preset path and recording the change in the equatorial diameter of each fruit over time to determine the fruit's development stage and optimal harvest period. This design achieves multi-functionality for the harvesting robot, significantly improving the equipment's annual utilization rate and agricultural data acquisition capabilities. Attached Figure Description

[0029] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be 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.

[0030] Figure 1 This is the overall flowchart of the present invention.

[0031] Figure 2 This is a flowchart of the automatic positioning and orientation calculation of key points of the fruit axis in this invention.

[0032] Figure 3 This is a schematic diagram of the K-means cluster classification of the present invention.

[0033] Figure 4 This is a schematic diagram illustrating the principle of equatorial diameter measurement in this invention.

[0034] Figure 5 This is a schematic diagram showing the diameter calculation results output by the algorithm model of this invention.

[0035] Figure 6 This is a schematic diagram showing the global multi-target harvesting planning results of the present invention.

[0036] Figure 7 This is a schematic diagram of the installation of the vision system for the harvesting robotic arm of the present invention.

[0037] Figure 8 This is a schematic diagram of the pose of each coordinate system in this invention. Detailed Implementation

[0038] The preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby providing a clearer and more explicit definition of the scope of protection of the present invention.

[0039] exist Figure 1 This invention provides a method for fruit axis pose estimation, pre-grading, and grasping pose solution in kiwifruit harvesting. It includes the following steps: Step S1, coarse fruit localization and classification: Based on the YOLOv11m target detection network, the fruit is quickly coarsely localized and classified according to its occlusion state. The target is categorized according to its spatial occlusion state, specifically into two classes: unoccluded independent fruit (KA) and occluded fruit (KF) blocked by leaves, branches, or other fruits. This provides a priority determination basis for multi-target harvesting sequence planning. Step S2, automatic localization of fruit axis key points and fruit axis orientation calculation: The binary mask of the fruit output by the instance segmentation model is obtained. The mask is preprocessed and its contour extracted. The preprocessing process utilizes the trained YOLOv11m network. The x-seg model infers from the input RGB image to obtain the original binary segmentation mask of the kiwi fruit. Gaussian blur is used to smooth the mask, eliminating jagged edges and holes. Then, a fixed threshold is applied for binarization, and all external contours are extracted from the binary image. The connected component with the largest area is selected, resulting in a mask image with complete shape, smooth edges, and removed minor noise. A least-squares ellipse fitting algorithm is used to fit an ellipse to the contour point set, obtaining the geometric parameters of the ellipse. Correction logic ensures the consistency of the semi-major axis and rotation angle. The pixel coordinates of the two endpoints of the major axis are calculated based on the ellipse's major axis parametric equation. Based on the prior knowledge that the calyx is located below the fruit in its natural hanging posture, the endpoint with the larger ordinate is marked as the calyx point. The endpoint with the smaller ordinate is marked as the fruit vertex. Wherein, "the endpoint with the larger ordinate is the calyx point" The discrimination rule relies on the biological prior of kiwifruit growing in the target harvesting area with the calyx facing down and the fruit apex facing up. Before harvesting, the system uses hand-eye calibration and camera pose constraints to ensure that the image's Y-axis is approximately parallel to the direction of gravity, thus stably corresponding to the semantics of the calyx and apex in the ordinate relationship. Combined with depth maps and camera intrinsic parameters, the system... , The two-dimensional pixel coordinates are transformed to the camera coordinate system to obtain the three-dimensional spatial coordinates of both. , Then calculate the unit direction vector of the fruit axis. , direction from point to The depth map and RGB map are established with a pixel-level correspondence through temporal alignment to ensure that key points of the same fruit are aligned. , The pixel coordinates can obtain the source depth value: Step S3, in Figure 3 In the middle, the picking order planning based on spatial clustering and category depth priority: using the fruit center in the camera coordinate system , K-Means clustering is performed on the coordinates to group nearby fruits in the same picking cluster; for fruits within each cluster, a picking sequence is generated according to the rule of "independent fruits first, and fruits of the same type from near to far in depth"; for different clusters, clusters containing independent fruits and closer to the camera are traversed first; in step S4, in Figure 4 and Figure 5 In the process of pre-grading and calculating the equatorial diameter of fruits: defining the equatorial center. Midpoint of the fruit axis With the geometric center of the mask Orthogonal projection point on the fruit axis line Weighted fusion: .in, From the calyx point With the apex The midpoint is determined, representing the geometric prior center of the fruit along the fruit axis. For the geometric center of the mask Orthogonal projection onto the fruit axis eliminates observation offsets perpendicular to the fruit axis. Weighted fusion preserves actual observation information while suppressing equatorial center offsets caused by attitude tilt or segmentation errors, improving the robustness of diameter calculations. Using the equatorial center as a reference, extending along the fruit axis normal vector to both sides to the mask boundary yields the two equatorial endpoints. , Calculate the pixel span from the pixel coordinates. Combined with the camera's equivalent focal length Reference depth with the center of the equator Calculate the physical diameter using the principle of similar triangles. At the same time , Transform to the camera coordinate system and calculate the true Euclidean distance between the two in 3D space. ;Will and The final equatorial diameter is obtained by weighted fusion. This is used as the diameter of the kiwi fruit (the basis for judging fruit size) and the basis for pre-grading at the picking end, to control the opening of the gripper and to achieve rough sorting of fruits by size: Step S5, gripping pose solution and execution: In this embodiment, the RGB-D camera is fixed to the end of the robotic arm in an eye-to-hand manner, and completes close-range perception of the same fruit sequence as the robotic arm moves. The direction of the fruit axis is used as the gripper feed direction, and the center of the equator is used as the gripper feed direction. To capture the target position, a fixed step size is used within the range of [-180°, 180°]. Enumerate all possible roll angles A set of candidate grasping poses is generated; the grasping poses satisfy two core constraints: the origin of the gripper TCP and the center of the equator. The coordinates in the base coordinate system coincide, and the gripper feed axis aligns with the fruit axis direction. Collinearity limits the gripper to three degrees of translation and two degrees of rotation (pitch / yaw), leaving only the roll angle around the gripper's feed axis as an undetermined degree of freedom. Inverse kinematics of the robot is solved for all candidate poses, and a comprehensive cost function is constructed to evaluate pose quality. This cost function comprehensively considers motion amplitude cost, joint constraint cost, motion singularity cost, and posture preference cost. Motion amplitude cost prioritizes poses with minimal joint motion to reduce picking time; joint constraint cost penalizes joint angles close to constraint thresholds to improve safety; motion singularity cost avoids wrist / elbow singular configurations through operability metrics; and posture preference cost ensures the gripper's final orientation is closer to the optimal geometric alignment with the fruit axis. The pose with the lowest cost is selected as the optimal grasping pose, which is then converted to the target pose in the robot's base coordinate system via a coordinate transformation link, controlling the robotic arm to perform the picking action.

[0040] The embodiments of the present invention are further illustrated by the following examples.

[0041] Example 1,

[0042] In step S1, the key points of the fruit axis are automatically located and the direction is calculated:

[0043] The specific implementation plan for this step is as follows:

[0044] 1. Fruit instance segmentation mask acquisition and preprocessing:

[0045] Obtain the fruit binary mask image output by a deep learning instance segmentation model (such as YOLOv11x-seg). Perform morphological closing and contour smoothing operations on the binary mask sequentially to remove isolated noise and edge burrs, resulting in a normalized binary mask.

[0046] 2. Extraction of the fruit's external contour:

[0047] A contour detection method based on the Suzuki-Abe boundary tracking algorithm (such as the findContours function in OpenCV) is used to extract all closed contours from the preprocessed binary mask. The contour with the largest area is selected as the outer contour point set of the target fruit, denoted as . .

[0048] 3. Fruit shape normalization based on least squares ellipse fitting:

[0049] The Fitzgibbon direct least squares ellipse fitting algorithm (such as the fitEllipse function in OpenCV) is used to fit an ellipse to the contour point set, obtaining the geometric parameters of the ellipse: center coordinates. Half-shaft length and rotation angle .in, and These correspond to the lengths of the major and minor semi-axis of the ellipse, respectively. The angle between the major axis and the horizontal axis of the image (clockwise is positive, and the range is [missing value]). ).

[0050] To ensure Always representing the major semi-axis, a correction logic is introduced: If Then exchange and The value, and the rotation angle Increase (mold deal with).

[0051] 4. Automatic positioning of key points on the fruit axis (calyx point and apex of the fruit):

[0052] Based on the corrected ellipse parameters, the coordinates of the two endpoints of the major axis are calculated using the equation of the major axis parameter. and :

[0053] Based on the biomorphological prior of kiwifruit in a naturally hanging state, where the calyx is at the bottom and the apex at the top, the endpoint with the larger ordinate is marked as the calyx point. The endpoint with the smaller ordinate is marked as the fruit vertex. This allows for the two-dimensional pixel coordinate positioning of key points on the fruit axis.

[0054] 5. Three-dimensional calculation of the fruit axis direction:

[0055] Obtain a depth map that is temporally aligned with the RGB image, and use the camera intrinsic parameter matrix to extract the calyx points. With the apex The two-dimensional pixel coordinates are transformed to the camera coordinate system to obtain the corresponding three-dimensional spatial coordinates. and Fruit axis direction vector Defined as by point to Unit vector: This vector represents the principal axis direction of the fruit in three-dimensional space, and is used for planning the subsequent grasping posture of the robotic arm.

[0056] Calyx point With the apex pixel coordinates Transform to camera coordinate system:

[0057]

[0058]

[0059]

[0060] in, For the coordinates of the optical center, Focal length For depth value,

[0061] fruit axis center point .

[0062] II. Picking sequence planning in step S2: First, utilize the fruit center obtained in step S1 in the camera coordinate system... , Coordinate components are used as input for K-Means clustering, employing Euclidean distance to group nearby fruits in physical space into the same picking cluster. Then, for fruits within each cluster, a picking sequence is generated according to the rule of "independent fruits first, and fruits of the same type from closest to furthest in depth." Between different clusters, clusters containing independent fruits and closer to the camera are traversed first. The global multi-target picking order result generated based on this framework is as follows: Figure 6 As shown in the figure, the numbers "#1" and "#2" indicate the fruit's position in the global picking sequence; the category labels "KA" and "KF" represent independent fruits and occluded fruits, respectively; the "SEG" marker indicates that the fruit has been included in the subsequent instance segmentation and pose estimation process. Ultimately, the global picking sequence presents a hierarchical decision-making result that traverses each cluster from near to far, prioritizing independent fruits within each cluster and then prioritizing nearby fruits (i.e., within a cluster, fruits are sorted by depth, and independent fruits are sorted by depth). Sort fruits from smallest to largest; if there are no independent fruits, sort all fruits by depth. Inter-cluster traversal order: prioritize the cluster containing independent fruits and the cluster with the smallest minimum depth value, providing the robotic arm with an efficient and safe basis for multi-target grasping decisions.

[0063] III. In step S3, the equatorial diameter is calculated online: In this embodiment, the equatorial center... ,in For the geometric center of the mask Orthogonal projection onto the fruit axis line. Projection calculation formula: Weighting coefficients The value is set to 0.5 (this can be optimized based on experiments). The physical significance of this weighted fusion design is as follows: It is entirely determined by the direction of the fruit axis and is not affected by the shape deviation of the segmentation mask, providing a stable geometric prior. Projecting the masked observation center onto the fruit axis preserves the actual observation information while eliminating the offset component perpendicular to the fruit axis. Weighted fusion of these two methods yields a robust equatorial center estimate even when the fruit's orientation is tilted or the segmentation mask is imperfect.

[0064] After obtaining the equatorial center, along the direction of the normal vector of the fruit axis (perpendicular to) And in the direction within the mask plane, search pixel by pixel to both sides until the mask boundary is encountered, obtaining the two equatorial endpoints. , The pixel coordinates. Calculate the pixel span. (Pixel distance), equivalent focal length Reference depth Take the center of the equator Components. Projected diameter: At the same time, , Transform to the camera coordinate system to obtain the corresponding 3D points , Calculate the three-dimensional Euclidean distance between two points: Final diameter , The value is set to 0.5. A dual-dimensional verification mechanism is used here for diameter calculation, which effectively compensates for systematic errors caused by the single projection method, significantly improving the stability and accuracy of diameter calculation. As a basis for pre-grading at the harvesting end, it is used to control the opening of the grippers and to achieve coarse sorting of fruits by size (for example, in this embodiment, the algorithm sets two threshold levels to divide kiwifruit into three grades: those with an equatorial diameter of less than 40.0 mm are small fruits, those with a diameter greater than or equal to 40.0 mm and less than 55.0 mm are medium fruits, and those with a diameter greater than or equal to 55.0 mm are large fruits). The final commodity grade determination is still based on the single fruit weight and sensory / physicochemical index system established in GB / T 40743-2021 "Kiwifruit Quality Grades" as the arbitration basis.

[0065] Experimental verification: 15 groups of kiwi fruit samples were collected, and the average absolute error between the visually predicted diameter and the actual measured value by vernier calipers was 0.85 mm.

[0066] IV. In step S4, the pose is captured and solved.

[0067] The specific implementation plan for this step is as follows:

[0068] 1. Kiwi fruit position Solution:

[0069] This embodiment adopts an "eye-in-hand" configuration, with the depth camera 3 fixed to the end of the jibular robotic arm 2 and positioned in conjunction with the end effector 1. Figure 7 As shown. The five defined coordinate systems are: kiwi coordinate system {M}, camera coordinate system {C}, flange coordinate system {T}, gripper tool coordinate system {T}, and collaborative arm base coordinate system {B}. The pose diagrams of each coordinate system are shown below. Figure 8 As shown. The pose matrix of the kiwi fruit in the cooperative arm base coordinate system is represented as follows. It requires a series of coordinate transformations to obtain the coordinates (kiwi coordinate system  camera coordinate system  flange coordinate system  base coordinate system), that is: .

[0070] Let be the global pose matrix of the kiwi fruit relative to the base coordinate system.

[0071] This is the real-time pose matrix of the flange relative to the base coordinate system (output in real time by the robotic arm).

[0072] The fixed homogeneous transformation matrix of the camera relative to the flange (solved by hand-eye calibration of the camera coordinate system relative to the flange coordinate system). (i.e., the extrinsic parameter matrix of the camera → flange)

[0073] The kiwifruit's pose relative to the camera is the observation matrix (obtained from the camera's intrinsic parameter matrix).

[0074] 2. Generating harvesting postures without damage:

[0075] To achieve non-destructive harvesting of kiwifruit, two core constraints must be met: the origin of the gripper's TCP (Trip Control Point) must coincide with the equatorial center of the kiwifruit; and the axis of the gripper must align with the kiwifruit's fruit axis. Full alignment ensures the grippers grasp the fruit from the bottom along the fruit axis. The gripping posture generation process includes four core steps: target position determination, feed direction determination, candidate posture enumeration, and optimal posture selection.

[0076] The target location was determined to be the center of the equator. Coordinates in the base coordinate system The feed direction is the same as that used in manual harvesting, with the gripper Z-axis aligned with the fruit axis. Alignment and collinearity, with the grippers approaching from the calyx end. Along the fruit axis towards the top of the fruit Direction. Candidate posture enumeration: After determining the position and feed direction, the gripper still has a degree of freedom around its own Z-axis (roll angle). The existence of this degree of freedom means that there are multiple possible gripper postures under the same gripping direction. After enumeration and sampling, the optimal posture is selected. In this embodiment... Within a range of fixed steps Enumerate all possible roll angles A total of 24 candidate grasp poses were generated. .(Note: The direction sign for the fruit's approach, with a value of Each candidate pose is solved using inverse kinematics to obtain the corresponding combination of joint angles. In this embodiment, inverse kinematics is performed on each candidate pose, yielding a maximum of 11 feasible solutions. The optimal pose is then determined by constructing a comprehensive cost function to evaluate the pose quality, selecting the pose with the lowest cost as the final optimal grasping pose. The cost function comprehensively considers four core factors: range of motion, joint limitations, motion singularity, and pose preference. The formula is as follows:

[0077]

[0078] : Cost of movement amplitude

[0079] Joint limiting cost

[0080] The Cost of Singularity

[0081] Posture preference cost

[0082] in ~ This is a weighting coefficient, which can be adjusted according to actual needs.

[0083] Not limited to this, any variations or substitutions conceived without inventive effort should be included within the scope of protection of this invention. Therefore, the scope of protection of this invention should be determined by the scope defined in the claims.

Claims

1. A method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting, characterized in that, Includes the following steps: Step S1, coarse fruit localization and coarse classification: Based on the YOLO target detection network, the fruit is quickly and coarsely located and classified according to its occlusion status. The target is classified according to the spatial occlusion status of the fruit, specifically into two categories: unoccupied independent fruit (KA) and occluded fruit (KF) blocked by leaves, branches, or other fruits. This provides a priority determination basis for multi-target picking sequence planning. Step S2, automatic positioning of key points on the fruit axis and calculation of the fruit axis direction: The binary mask of the fruit is obtained through an instance segmentation model. The binary mask is preprocessed and its contour is extracted. Then, an ellipse is fitted. Based on the endpoints of the major axis of the ellipse and combined with the prior knowledge of the fruit's natural growth posture, the calyx point is automatically located. With the apex The two-dimensional pixel coordinates; combined with the depth map and camera intrinsic parameters, the... , Transform pixel coordinates to camera coordinates and calculate the unit direction vector of the fruit axis. ; Step S3, picking order planning based on spatial clustering and category depth priority: Spatial clustering is performed based on the fruit center coordinates, and fruits that are physically close to each other are divided into the same picking cluster. For the fruits within each cluster, the picking order within the cluster is generated according to the rule of "independent fruits first, and fruits of the same type from near to far in depth". Step S4, Pre-grading and calculation of fruit equatorial diameter: Define the center of the equator Midpoint of the fruit axis With the geometric center of the mask Orthogonal projection point on the fruit axis line Weighted fusion, by preserving actual observation information while suppressing center offset caused by attitude tilt or segmentation errors, improves the robustness of the measurement. Using the equatorial center as a reference, the mask boundary is sampled along the fruit axis normal vector direction to obtain the two equatorial endpoints, and the projected diameter is calculated respectively. With three-dimensional space diameter The final equatorial diameter is obtained by weighted fusion of the two. ,Will As a basis for pre-grading of kiwifruit fruit diameter and picking end, it is used to control the opening of the gripper and realize the coarse sorting of fruits by size. Step S5, Capture Pose Solving and Execution: Centered on the equator To capture the target position, along the fruit axis direction For the gripper feed direction, a set of candidate gripping poses is generated by enumerating candidate roll angles. Then, the optimal gripping pose is selected by solving inverse kinematics and evaluating the comprehensive cost function. The pose is then transformed to the base coordinate system of the collaborative robotic arm via a coordinate transformation link, and the robotic arm is controlled to perform the picking action.

2. The method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting according to claim 1, characterized in that... In step S2, the preprocessing process involves using the trained YOLO11x-seg model to infer the input RGB image and obtain the original binary segmentation mask of the kiwi fruit. Gaussian blur is then used to smooth the mask, eliminating jagged edges and holes. Finally, a fixed threshold is used for binarization, and all external contours are extracted from the binary image. The connected component with the largest area is selected, thereby obtaining a mask image with complete shape, smooth edges, and removal of minor noise.

3. The method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting according to claim 1, characterized in that... The ellipse fitting in step S2 uses Fitzgibbon ellipse fitting, which is the direct least squares ellipse fitting algorithm, to fit the contour point set to an ellipse and obtain the geometric parameters of the ellipse, namely the center coordinates, the length of the semi-axis, and the rotation angle. The consistency between the major semi-axis and the rotation angle is ensured by correction logic. The correction logic is as follows: if the minor semi-axis is greater than the major semi-axis, the major and minor semi-axis are swapped and the rotation angle is increased by 90°. The increased rotation angle is then processed with a modulus of 180°.

4. The method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting according to claim 1, characterized in that... The fruit vertex described in step S2 The rule for determining the two-dimensional pixel coordinates is as follows: compare the ordinates of the two endpoints of the major axis of the ellipse, and mark the endpoint with the larger ordinate as the calyx point. The endpoint with the smaller ordinate is marked as the fruit vertex. The discrimination rule for the larger endpoint of the ordinate, representing the calyx point M, relies on the biological prior of the kiwifruit growing in the target harvesting area with the calyx end facing down and the fruit apex facing up. Before harvesting, the system uses hand-eye calibration and camera pose constraints to ensure that the image Y-axis is approximately parallel to the direction of gravity, thus ensuring that the relative sizes of the ordinates stably correspond to the semantics of the calyx and fruit apex. Combining the depth map and camera intrinsic parameters, the system... , The two-dimensional pixel coordinates are transformed to the camera coordinate system to obtain the three-dimensional spatial coordinates of both. , Then calculate the unit direction vector of the fruit axis. , direction from point to In this process, the depth map and the RGB map are aligned temporally to establish a pixel-level correspondence, ensuring that key points of the same fruit are aligned. , The pixel coordinates can obtain the source depth value.

5. The method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting according to claim 1, characterized in that... The spatial clustering described in step S3 uses the K-Means clustering algorithm, which utilizes the position of the fruit center in the camera coordinate system. , K-Means clustering is performed on the coordinates to divide nearby fruits in the physical space into the same picking cluster; for fruits within each cluster, a picking sequence is generated according to the rule of "independent fruits first, and fruits of the same type from near to far in depth"; for different clusters, the clusters containing independent fruits and closer to the camera are traversed first.

6. The method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting according to claim 1, characterized in that... In step S4, the center of the equator is defined. Midpoint of the fruit axis With the geometric center of the mask Orthogonal projection point on the fruit axis line The weighted fusion, its The calculation formula is: , in, From the calyx point With the apex The midpoint is determined, representing the geometric prior center of the fruit along the fruit axis. For the geometric center of the mask Orthogonal projection onto the fruit axis eliminates observation offsets perpendicular to the fruit axis. These are the preset weighting coefficients. In step S4, using the center of the equator as a reference, the vector is extended to both sides along the direction of the fruit axis normal vector to the mask boundary to obtain the two ends of the equator. , Calculate the pixel span from the pixel coordinates. Combined with the camera's equivalent focal length Reference depth with the center of the equator Calculate the physical diameter using the principle of similar triangles. And among them and This is the equivalent focal length of the camera.

7. The method for fruit axis orientation, pre-grading, and grasping pose determination during kiwifruit harvesting according to claim 1, characterized in that... The three-dimensional spatial diameter mentioned in step S4 The 3D Euclidean distance between the two points is calculated by transforming the pixel coordinates of the two equatorial endpoints to the camera coordinate system. The method involves... , Transform to the camera coordinate system and calculate the true Euclidean distance between the two in 3D space. ;Will and The final equatorial diameter is obtained by weighted fusion. It is used as the basis for judging the diameter of kiwifruit (the basis for judging fruit size) and the basis for pre-grading at the picking end, so as to control the opening of the gripper and realize the coarse sorting of fruits by size.

8. The method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting according to claim 1, characterized in that... In step S5, the candidate roll angle is within the range of [-180°, 180°] with a fixed step size. Enumerate to generate a set of candidate grab poses.

9. The method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting according to claim 1, characterized in that... The comprehensive cost function described in step S5 includes at least one or more of the following four items: motion amplitude cost, joint limitation cost, motion singularity cost, and posture preference cost.

10. The method for estimating the fruit axis posture, pre-grading, and solving the grasping pose during kiwifruit harvesting according to claim 1, characterized in that... The coordinate transformation link mentioned in step S5 includes the sequential transformation between the kiwi coordinate system {M}, the camera coordinate system {C}, the flange coordinate system {T}, the gripper tool coordinate system {G}, and the collaborative robot arm base coordinate system {B}.

Images