A mango picking robot system and method for high branch dense orchard

Abstract

The application provides a mango picking robot system and method for high-branch dense orchard, and relates to the technical field of agricultural robots, and comprises: a sensor data acquisition module, which is used to obtain processed original multi-sensor data; a multi-modal perception inference module, which is used to jointly detect mango fruit bodies and fruit stems on image data to obtain a plurality of target mango data; an active view reconstruction module, which is used to actively reconstruct the view of each target mango data and jointly decide and screen the pickability, sort the priority by using a candidate target comprehensive priority function, and generate a target picking sequence; a task planning and scheduling module, which is used to model the environment by using the current priority target and point cloud data, and construct a local planning field; plan a path by using the local planning field to obtain an executable trajectory; and an actuator output module, which is used to convert the executable trajectory into picking control instructions. The application improves the picking accuracy of the mango picking robot.

Description

Technical Field

[0001] This invention relates to the field of agricultural robot technology, and more specifically, to a mango harvesting robot system and method for high-branch, densely irrigated orchards. Background Technology

[0002] Mangoes are a high-value tropical fruit with a short ripening window, making manual harvesting insufficient to meet timely requirements. Robotic systems capable of autonomously harvesting mangoes in natural orchard environments have become an important development direction for smart orchard equipment. Currently, mango harvesting robot systems primarily use shortest path planning to navigate to the vicinity of the target tree, then utilize two-dimensional object detection to determine the approximate boundary of the fruit, relying on the fruit's center point to approach and harvest the mango. However, mangoes are often partially obscured by leaves, branches, immature fruits, or adjacent ripe fruits within the tree canopy. Traditional two-dimensional object detection can only provide the approximate boundary of the fruit, making it difficult to consistently determine the position of the fruit stalk and the optimal cutting point. Furthermore, mangoes have a wide size distribution, with significant differences in stalk thickness, angle, and attachment direction. Traditional mango harvesting methods often result in damage to the mango fruit, leading to economic losses. Summary of the Invention

[0003] The present invention aims to solve at least one of the above-mentioned problems.

[0004] To address the aforementioned problems, this invention provides a mango harvesting robot system and method for orchards with high branches and dense irrigation.

[0005] In a first aspect, the present invention provides a mango harvesting robot system for high-branch, densely irrigated orchards, comprising: The sensor data acquisition module is used to perform timestamp calibration and sensor synchronization processing on the raw multi-sensor data to obtain the processed raw multi-sensor data, wherein the processed raw sensor data includes image data and point cloud data. The multimodal perception inference module is used to perform joint detection of mango fruit body and stem on the image data based on a lightweight target detection network to obtain multiple target mango data. The active perspective reconstruction module is used to perform active perspective reconstruction and joint decision-making screening of the target mango data to obtain the screened target mango data; the candidate target comprehensive priority function is used to prioritize the screened target mango data to generate a target picking sequence. The task planning and scheduling module is used to obtain the current priority target based on the target picking sequence; to perform environmental modeling using the current priority target and the point cloud data to construct a local planning field; and to perform path planning using the local planning field to obtain an executable trajectory. The actuator output module is used to convert the executable trajectory into picking control commands based on the pose-force collaborative control algorithm, wherein the picking control commands are used to control the robotic arm and the end effector to perform cutting.

[0006] Optionally, the target mango data includes mature mango detection data and the corresponding region connecting the fruit stem and the fruit body. The image data is then subjected to joint detection of the mango fruit body and fruit stem using a lightweight target detection network to obtain multiple target mango data sets, including: Target detection is performed on the image data to obtain multiple candidate mango detection data; The maturity of each candidate mango was comprehensively evaluated to obtain a comprehensive maturity score. The candidate mango detection data are filtered based on the comprehensive maturity score to obtain the mature mango detection data; When the confidence level of the fruit stem region in the mature mango detection data is higher than a preset value, the candidate mango detection data is input into the instance segmentation network to obtain a fine-grained fruit stem mask. The fine-grained fruit stalk mask is refined, noise filtered, and skeletalized to obtain the connection area between the fruit stalk and the fruit body.

[0007] Optionally, the overall maturity score is expressed as: , in, The overall maturity score for the i-th mango is given. Let i be the color ripeness feature component of the i-th mango. Let be the texture maturity feature component of the i-th mango. Let be the near-infrared or near-infrared surrogate feature component of the i-th mango. For the i-th mango, the historical time-series consistency component is... , , and These are the maturity assessment weighting coefficients, and .

[0008] Optionally, the step of performing active perspective reconstruction and joint decision-making screening on the target mango data to obtain the screened target mango data includes: The target mango data are observed from a main perspective to generate multiple candidate observation poses. Each candidate observation pose is evaluated for viewpoint gain to obtain viewpoint gain. The candidate observation poses are filtered using the aforementioned viewpoint gain to obtain the target observation pose; The collectability of each target mango data is predicted by the target observation pose to obtain the corresponding collectability probability. The collectability probability is expressed as: , in, Let be the collectability probability of the i-th mango. For the Sigmoid function, The feature vectors of the target observation pose and the target mango data. and These are the parameters of the feasible probability model; The target mango data is filtered using the available probability to obtain the filtered target mango data.

[0009] Optionally, the viewpoint benefit is expressed as:

[0010] , in, For the j-th candidate observation pose, For the benefits of the aforementioned perspective, For overall visibility of the target, For the fruit stalk to be significant, For the integrity of depth information, For travel time or energy costs, To mitigate the risk of collisions during localized movement, , , , and These are the corresponding perspective benefit weights.

[0011] Optionally, the candidate target comprehensive priority function is expressed as: , in, Let i be the priority of the i-th mango. The overall maturity score for the i-th mango is given. Let be the collectability probability of the i-th mango. For visibility score, To cover up punishment, To mitigate the risk of damage, To account for the overall path cost, , , , , and Each of these represents a configurable weight for its corresponding priority level.

[0012] Optionally, the step of constructing a local planning field by modeling the environment using the current priority target and the point cloud data includes: The environment of the current priority target within a preset range is reconstructed using the point cloud data to obtain an obstacle avoidance collision body; The local planning field is constructed using the point cloud data and the obstacle avoidance collision body.

[0013] Optionally, the step of performing path planning through the local planning field to obtain an executable trajectory includes: Multiple feasible coarse paths are generated through the local planning field; The feasible coarse-grained paths are optimized using a trajectory cost function to obtain the executable trajectory; The trajectory cost function is expressed as: , in, For the cost of trajectory, This is the feasible coarse path. For the trajectory length, The cost of colliding with obstacles, Due to energy consumption and the cost of joint movement, As a cost to trajectory smoothness, Due to the time cost of execution, , , , and These are the corresponding trajectory cost weights.

[0014] Optionally, it also includes a damage risk assessment module, which is used to monitor the picking process in real time using the actuator output module to obtain mango picking monitoring data; and to obtain a clamping damage risk index through the mango picking monitoring data, wherein the clamping damage risk index is used to correct the picking control command.

[0015] Secondly, the present invention provides a mango harvesting robot method for high-branch, densely irrigated orchards, applied to the aforementioned mango harvesting robot system for high-branch, densely irrigated orchards, the mango harvesting robot method comprising: The original multi-sensor data is time-stamped and synchronized with the sensors to obtain the processed original multi-sensor data, wherein the original sensor data includes image data and point cloud data. Based on a lightweight target detection network, the image data is used to perform joint detection of mango fruit body and stem to obtain multiple target mango data. Active perspective reconstruction and joint decision-making screening of the target mango data are performed to obtain the screened target mango data; the candidate target comprehensive priority function is used to prioritize the screened target mango data to generate a target picking sequence. The current priority target is obtained based on the target picking sequence; environmental modeling is performed using the current priority target and the point cloud data to construct a local planning field; path planning is performed using the local planning field to obtain an executable trajectory; Based on the pose-force collaborative control algorithm, the executable trajectory is converted into picking control commands, wherein the picking control commands are used to control the robotic arm and end effector to perform cutting.

[0016] The beneficial effects of the mango harvesting robot system and method for high-branch, densely irrigated orchards of the present invention are as follows: The sensor data acquisition module performs timestamp calibration and sensor synchronization processing on the raw multi-sensor data, eliminating the temporal deviation between image data and point cloud data at the time of acquisition, thus providing a guarantee for subsequent data processing. The multimodal perception inference module uses a lightweight target detection network to jointly detect mango fruit bodies and stems in the image data, obtaining multiple target mango data simultaneously. This avoids the problem of inaccurate harvesting point positioning caused by only detecting the center point of the fruit and ignoring the position of the stem in traditional solutions. The active perspective reconstruction module performs active perspective reconstruction and joint decision-making screening for each target mango data, combining observation perspective and harvestability for screening. Local reachability priors are introduced during perspective selection, improving the feasibility of the harvesting plan. A comprehensive priority function for candidate targets is used for priority ranking, dynamically optimizing the harvesting order based on multi-dimensional factors. The task planning and scheduling module performs environmental modeling using the current priority targets and point cloud data, providing accurate spatial constraint information for path planning and constructing a local planning field. By using a local planning field for path planning, an executable trajectory is obtained, effectively avoiding collisions between the robotic arm and branches and leaves during the approach to the target, thus improving the reliability and success rate of the harvesting operation. The actuator output module, based on a pose-force collaborative control algorithm, converts the executable trajectory into harvesting control commands, controlling the robotic arm and end effector to perform cutting, significantly reducing the fruit damage rate during the harvesting process. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of a mango harvesting robot system for high-branch, densely irrigated orchards, according to an embodiment of the present invention. Figure 2 This is a flowchart illustrating a mango harvesting robot method for high-branch, densely irrigated orchards according to an embodiment of the present invention. Detailed Implementation

[0018] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Although some embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the present invention. It should be understood that the accompanying drawings and embodiments of the present invention are for illustrative purposes only and are not intended to limit the scope of protection of the present invention.

[0019] It should be understood that the various steps described in the method embodiments of the present invention may be performed in different orders and / or in parallel. Furthermore, the method embodiments may include additional steps and / or omit the steps shown. The scope of the present invention is not limited in this respect.

[0020] The term "comprising" and its variations as used herein are open-ended inclusions, meaning "including but not limited to"; the term "based on" means "at least partially based on"; the term "one embodiment" means "at least one embodiment"; and the term "another embodiment" means "at least one additional embodiment". Definitions of other terms will be given in the following description.

[0021] It should be noted that the terms "one" and "more" used in this invention are illustrative rather than restrictive. Those skilled in the art should understand that, unless otherwise expressly indicated in the context, they should be understood as "one or more".

[0022] like Figure 1 As shown, a mango harvesting robot system for high-branch, densely irrigated orchards includes: The sensor data acquisition module is used to perform timestamp calibration and sensor synchronization processing on the raw multi-sensor data to obtain the processed raw multi-sensor data, wherein the raw sensor data includes image data and point cloud data.

[0023] Specifically, raw multi-sensor data is acquired through a multi-modal sensing module, which includes at least one or more combinations of RGB cameras, RGB-D cameras, binocular cameras, near-infrared / multispectral cameras, LiDAR, IMU, force sensors, end-effector miniature cameras, or tactile sensors. Since the sampling frequencies of each sensor differ and data transmission is delayed, direct fusion would lead to spatiotemporal misalignment. Therefore, using the IMU's high-frequency clock as a reference, the exposure times of the depth camera and RGB camera are synchronized via GPIO trigger signals to ensure temporal alignment between image frames and point cloud frames. Each acquired frame is timestamped to milliseconds, and linear interpolation is used to align data of different frequencies to a unified time reference. The intrinsic and extrinsic parameter matrices of the depth camera and RGB camera are pre-calibrated using a checkerboard calibration method to achieve precise mapping between image pixels and point cloud spatial coordinates.

[0024] In some more specific embodiments, a LiDAR (Light Detection and Ranging) system outputs 3D point cloud data for canopy contour reconstruction and chassis navigation. A wide-angle RGB camera outputs a W×H×3 resolution color image for tree location identification and coarse screening. An inertial measurement unit (IMU) outputs attitude quaternions and accelerations for chassis attitude leveling. An RGB-D camera simultaneously outputs color and depth images. A binocular stereo camera calculates depth using parallax. A near-infrared / multispectral camera outputs multichannel spectral images for fine maturity assessment. An end effector miniature camera outputs close-range images with a resolution of 640×480. A six-dimensional force sensor outputs force and torque vectors. A thin-film pressure array outputs a pressure distribution matrix with a sampling frequency ≥100 Hz.

[0025] The multimodal perception inference module is used to perform joint detection of mango fruit body and stem on the image data based on a lightweight target detection network, so as to obtain multiple target mango data.

[0026] Specifically, a lightweight object detection network first simultaneously detects fruits and stems in image data, then solves for shearing points through instance segmentation, skeleton line extraction, and 3D fitting, no longer approximating the fruit center as the picking location. The lightweight object detection network employs an improved YOLO series network or the MobileNet-SSD architecture. The YOLO series continuously improves accuracy while maintaining real-time performance through iterative development of the backbone network and detection head structure; MobileNet-SSD, with depthwise separable convolutions at its core, achieves a high-efficiency balance between accuracy and speed in resource-constrained scenarios.

[0027] The active perspective reconstruction module is used to perform active perspective reconstruction and joint decision-making screening of the target mango data to obtain the screened target mango data; the candidate target comprehensive priority function is used to prioritize the screened target mango data to generate a target picking sequence.

[0028] Specifically, addressing the issue that fruit stalks are often invisible from the main viewpoint in natural orchards, this study automatically generates several candidate observation poses based on the current occlusion score, edge confidence, and consistency with historical observations. These candidate poses are then filtered using acceptability prediction to obtain the target observation pose. Acceptability prediction is then performed on the target mango data under the target observation pose, and data that does not meet the requirements is filtered to obtain the final target mango data.

[0029] The task planning and scheduling module is used to obtain the current priority target based on the target picking sequence; to perform environmental modeling using the current priority target and the point cloud data to construct a local planning field; and to perform path planning using the local planning field to obtain an executable trajectory.

[0030] Specifically, taking the cutting point of the fruit stalk of the current priority target as the center, local point cloud data within a preset radius (e.g., 0.5m) is extracted to construct a local planning field for environmental modeling. Within the local planning field, i.e., the local canopy space, the system adopts a three-stage strategy of environmental modeling, coarse path generation, and trajectory optimization.

[0031] The actuator output module is used to convert the executable trajectory into picking control commands based on the pose-force collaborative control algorithm, wherein the picking control commands are used to control the robotic arm and the end effector to perform cutting.

[0032] In some more specific embodiments, the robotic arm can be a single-arm, dual-arm, or foldable multi-joint robotic arm structure. The robotic arm has 6 degrees of freedom or more to meet the requirements for approaching the complex postures inside the mango canopy. The end effector of the robotic arm connects to an end effector, and different types of end effectors can be replaced via quick-change flanges. The end effector includes a compliant gripping and directional cutting end effector, which comprises a guide cover, a compliant wrapping and clamping mechanism, a fruit stalk guide groove, a directional cutting mechanism, a micro-vision sensing mechanism, a force / pressure sensing mechanism, a buffer support component, and a cleaning and drainage assembly. The guide cover guides the separation of branches and leaves when entering local gaps in the canopy and prevents the fruit surface from being directly scratched by sharp parts. The compliant wrapping and clamping mechanism can use elastic fingertips, flexible bags, wrapping films, or soft-hard composite fingertips. The fruit stalk guide groove constrains the fruit stalk to the cutting blade's working area. The directional cutting mechanism can be in the form of a swing shear, a ring shear, a scissor, or a micro-saw. The miniature visual sensing mechanism is preferably an embedded miniature camera, used for close-range judgment of the fruit's covering state, stem position, and cutting result. The force / pressure sensing mechanism can employ a thin-film pressure array, a six-dimensional force sensor, or an optical tactile module. Mangoes may release juice after stem cutting, affecting subsequent recognition and grasping. A cleaning and drainage component is added to the end effector for automatic cleaning between multiple harvesting cycles. The goal is to first stabilize the fruit, then perform a low-disturbance cut along the stem direction, allowing the fruit to smoothly transfer into the storage channel in a short time. When the robotic arm moves to the designated position, the end effector receives the harvesting control command and approaches the target fruit at a low speed. After the guide cover pushes a small number of leaves aside, the compliant gripping mechanism unfolds along the two or three sides of the fruit, forming a low-pressure covering. When the contact area reaches a threshold and the local pressure distribution is uniform, the stem guide groove constrains the stem to the vicinity of the cutter, and then the stem-cutting mechanism completes the cut in a preset direction. At the moment of cutting, the buffer support component bears the instantaneous weight change of the fruit, preventing excessive fruit swaying, and finally the fruit is smoothly transferred to the buffer support component.

[0033] In some more specific embodiments, the harvesting control commands are also used to control the fruit transport and storage module. While the robotic arm and end effector are performing the cutting operation, the fruit transport and storage module is simultaneously moved to a designated position. The fruit transport and storage module includes buffer slides, flexible fruit baskets, vacuum-padded compartments, or independently compartmented turnover boxes to reduce the risk of instantaneous collisions and crushing after harvesting. For high-grade fresh mangoes, a single-fruit independent storage strategy can be adopted.

[0034] Specifically, based on the pose tracking control strategy, the robotic arm is driven to move along the executable trajectory to the corresponding pose. The executable trajectory is discretized into intermediate pose points, and the corresponding joint angle sequence is obtained through inverse kinematics. A PID controller is used for closed-loop control of each joint. When the end effector of the robotic arm approaches or contacts the surface of the mango fruit, the system switches to the pose-force cooperative control algorithm mode, using impedance relationships to achieve compliant interaction, as shown below: , in, , , Let these represent the desired inertia matrix, damping matrix, and stiffness matrix, respectively. This indicates the deviation of the end effector from the desired contact pose. Indicates the actual external force at the end. Indicates the desired contact force. and They are respectively The system employs first-order and second-order guides. Higher pose stiffness is set during the approach phase, lower normal stiffness during the wrapping phase, and improved tangential stability during the cutting phase. Once the end effector grippers stably hold the mango fruit, the system enters the cutting execution phase. At this point, the desired contact force switches to a cutting feed force (such as a thrust along the fruit stem axis), and the impedance controller maintains a constant cutting force to ensure successful stem severing. Since the mango stem direction has significant geometric meaning, the end effector should not approach randomly from any direction. The system constructs an approach posture constraint set based on the local tangent vector of the stem skeleton line, the target surface normal vector, and the current gripper posture. This ensures that the end effector guide shield first forms a safe angle with the outer side of the fruit, and then completes the wrapping and cutting along the allowed direction of the stem. This reduces interference with adjacent branches and leaves, and also minimizes torsional disturbance to the fruit during stem cutting.

[0035] In some more specific embodiments, a mango harvesting robot system is applied to a mango harvesting robot, which includes a robotic arm, an end effector, a fruit transfer and storage module, a mobile chassis, and a posture leveling and lifting assembly. The mobile chassis is preferably any of a four-wheel differential, four-wheel steering, tracked, or wheel-leg hybrid chassis, used for autonomous movement in orchard paths, under-tree passages, and field areas. The chassis may integrate a battery pack, drive motor, obstacle avoidance radar, inertial measurement unit, and emergency braking mechanism. The posture leveling and lifting assembly is mounted on top of the mobile chassis and can adjust the robot's workspace vertically or in the pitch direction according to the tree canopy height, the spatial distribution of the target fruit, and the optimal operating range of the robotic arm. For mangoes on high branches, the lifting assembly can raise the robotic arm base closer to the fruit layer, thereby reducing the decrease in stiffness and the risk of branch collision caused by a large upward lift.

[0036] In this embodiment, the sensor data acquisition module performs timestamp calibration and sensor synchronization processing on the raw multi-sensor data, eliminating the temporal discrepancy between image data and point cloud data at the time of acquisition, thus ensuring the smooth processing of subsequent data. The multimodal perception inference module utilizes a lightweight target detection network to jointly detect mango fruit bodies and stems from the image data, simultaneously obtaining multiple target mango data points. This avoids the problem of inaccurate picking point positioning caused by only detecting the center point of the fruit and ignoring the stem position in traditional solutions. The active perspective reconstruction module performs active perspective reconstruction and joint decision-making screening for each target mango data point, combining observation perspective and accessibility for screening. Local reachability priors are introduced during perspective selection, improving the feasibility of the picking plan. A comprehensive priority function for candidate targets is used for priority ranking, dynamically optimizing the picking order based on multi-dimensional factors. The task planning and scheduling module performs environmental modeling using the current priority targets and point cloud data, providing accurate spatial constraint information for path planning and constructing a local planning field. By using a local planning field for path planning, an executable trajectory is obtained, effectively avoiding collisions between the robotic arm and branches and leaves during the approach to the target, thus improving the reliability and success rate of the harvesting operation. The actuator output module, based on a pose-force collaborative control algorithm, converts the executable trajectory into harvesting control commands, controlling the robotic arm and end effector to perform cutting, significantly reducing the fruit damage rate during the harvesting process.

[0037] Optionally, the target mango data includes mature mango detection data and the corresponding region connecting the fruit stem and the fruit body. The image data is then subjected to joint detection of the mango fruit body and fruit stem using a lightweight target detection network to obtain multiple target mango data sets, including: Target detection is performed on the image data to obtain multiple candidate mango detection data; The maturity of each candidate mango was comprehensively evaluated to obtain a comprehensive maturity score. The candidate mango detection data are filtered based on the comprehensive maturity score to obtain the mature mango detection data; When the confidence level of the fruit stem region in the mature mango detection data is higher than a preset value, the candidate mango detection data is input into the instance segmentation network to obtain a fine-grained fruit stem mask. The fine-grained fruit stalk mask is refined, noise filtered, and skeletalized to obtain the connection area between the fruit stalk and the fruit body.

[0038] Specifically, a lightweight object detection network is used to perform object detection on image data, performing multi-class detection on mangoes, stems, branches, and leaves in the images. The network outputs the bounding box location information of the mango in the image coordinate system and preliminary class confidence scores, resulting in multiple candidate mango detection data. The lightweight object detection network employs an improved YOLO series network or a MobileNet-SSD architecture.

[0039] A comprehensive maturity assessment was performed on each candidate mango detection data. Based on the candidate mango detection data, texture maturity feature components, color maturity feature components, near-infrared or near-infrared proxy feature components, and historical temporal consistency components were obtained to derive a comprehensive maturity score. Candidate mango detection data with a comprehensive maturity score greater than the maturity threshold were recorded as mature mango detection data; otherwise, the candidate mango detection data were discarded. The confidence level of the fruit stem region in the mature mango detection data was determined, and further fine-grained fruit stem masking was performed.

[0040] Because fruit stalks are typically thin and easily obscured in natural environments, direct detection is challenging. When the confidence level of the fruit stalk region in the mature mango detection data exceeds a preset value (indicating a preliminary detection of a suspected fruit stalk), to obtain a more precise cutting location, the system inputs the image region corresponding to the candidate mango detection data into an instance segmentation network (such as MaskR-CNN or YOLACT). The instance segmentation network performs pixel-level classification of the fruit stalk region, outputting a fine-grained fruit stalk mask. The fine-grained fruit stalk mask accurately obtains the outline of the fruit stalk, distinguishing the pixel boundaries between the fruit stalk and the background branches and leaves. The fine-grained fruit stalk mask undergoes thinning, noise filtering, and skeletonization to obtain the connection region between the fruit stalk and the fruit body. This connection region represents the centerline of the fruit stalk, the bifurcation point, and the connection area in contact with the fruit body. Thinning and noise filtering are used to remove isolated noise points in the mask using morphological opening operations, and to reduce the outline of the fruit stalk to a single-pixel-width centerline using a thinning algorithm. Skeletonization is used to extract the skeleton from the processed fruit stalk mask, generating a skeleton curve representing the direction of the fruit stalk. Connection region localization is used to represent the endpoints where the skeleton curve extends to the surface of the mango fruit; the region where these endpoints are located is the connection area between the fruit stalk and the fruit. The system combines depth maps or point clouds to perform 3D fitting of the fruit stalk centerline, obtaining the local tangential and normal directions, providing a basis for setting the posture of the stalk cutter.

[0041] In this optional embodiment, a lightweight object detection network is first used for multi-class detection. Then, an instance segmentation network is activated for the fruit stem region with high confidence, obtaining a fine-grained fruit stem mask. After refining, noise filtering, and skeletonization of the mask, the connection region between the fruit stem and the fruit body is extracted. This not only identifies ripe mangoes but also accurately segments small fruit stems in complex backgrounds and locates the connection point between the stem and the fruit body. This precise spatial location information serves as a key target point for subsequent robotic arm path planning and end effector cutting actions, effectively avoiding fruit damage and improving harvesting success rate.

[0042] Optionally, the overall maturity score is expressed as: , in, The overall maturity score for the i-th mango is given. Let i be the color ripeness feature component of the i-th mango. Let be the texture maturity feature component of the i-th mango. Let be the near-infrared or near-infrared surrogate feature component of the i-th mango. For the i-th mango, the historical time-series consistency component is... , , and These are the maturity assessment weighting coefficients, and .

[0043] Specifically, mango portion images are obtained from candidate mango detection data, and the average values ​​of the hue and saturation channels within that region are extracted. Set the target color of a fully ripe mango. The color maturity mapping function is constructed as follows: .

[0044] Texture features in candidate mango detection data are calculated using the gray-level co-occurrence matrix (GLCM), with a focus on extracting contrast and energy as the key indicators to obtain the texture maturity feature components. Mature mangoes typically have a smoother surface or a specific waxy sheen, exhibiting lower texture contrast and higher energy. If the system is equipped with a near-infrared (NIR) camera, the reflectance intensity image of the i-th mango in the near-infrared band (e.g., 850nm or 940nm) is directly acquired. Since changes in moisture and sugar content in ripe fruit lead to alterations in near-infrared absorptivity, the average gray value of this region is directly normalized and used as the reference value. If the system is equipped only with a standard RGB camera, a "surrogate feature" strategy is employed. Near-infrared features are approximated using the ratio of a specific green channel (G) to a red channel (R) in the RGB image, or a variant of the Normalized Difference Vegetation Index (NDVI). A multi-target tracking algorithm (such as the Kalman Filter or SORT algorithm) is used to locate the i-th mango in consecutive video frames, establishing its historical observation sequence. The maturity score sequence of this mango over the past K frames (e.g., the last 30 frames in the last 3 seconds) is obtained. The maturity score sequence is used to evaluate the consistency of candidate mango detection data in the current frame, yielding a historical temporal consistency component, which is used to introduce the maturity trend of the same target in adjacent moments or adjacent inspection cycles. The color maturity feature component is used to determine mango maturity by peel color, the texture maturity feature component is used to evaluate peel roughness, gloss, and surface spots, and the near-infrared or near-infrared surrogate feature component is used to characterize internal changes in the mango. The weight coefficients are not fixed but determined through offline training. A large amount of labeled mango sample data was collected, and the mapping relationship between feature vectors and real labels was constructed. The least squares method or genetic algorithm was used for regression fitting to obtain the optimal weight combination.

[0045] In this optional embodiment, the comprehensive evaluation of four dimensions can overcome the instability of a single feature in a complex orchard environment, and significantly improve the accuracy of mango maturity judgment.

[0046] Optionally, the step of performing active perspective reconstruction and joint decision-making screening on the target mango data to obtain the screened target mango data includes: The target mango data are observed from a main perspective to generate multiple candidate observation poses. Each candidate observation pose is evaluated for viewpoint gain to obtain viewpoint gain. The candidate observation poses are filtered using the aforementioned viewpoint gain to obtain the target observation pose; The collectability of each target mango data is predicted by the target observation pose to obtain the corresponding collectability probability. The collectability probability is expressed as: , in, Let be the collectability probability of the i-th mango. For the Sigmoid function, The feature vectors of the target observation pose and the target mango data. and These are the parameters of the feasible probability model; The target mango data is filtered using the available probability to obtain the filtered target mango data.

[0047] Specifically, firstly, the target mango data after maturity screening is observed from a primary viewpoint. The primary viewpoint refers to the default observation pose of the current camera, typically the camera pose when the robotic arm's end effector is in a natural standby state. From the primary viewpoint, the system acquires the 3D coordinates of the target mango and point cloud data of its surrounding environment. Based on these 3D coordinates, N candidate observation poses are generated within the robotic arm's reachable workspace, centered on the target mango. Each candidate observation pose is defined by six degrees of freedom parameters, including the camera's spatial coordinates in the robotic arm's base coordinate system, the camera's pitch angle, yaw angle, and roll angle. The viewpoint gain is evaluated for each candidate observation pose, and its viewpoint gain is calculated. The viewpoint gain quantifies the improvement in observation quality compared to the current primary viewpoint. When the viewpoint gain exceeds a viewpoint gain threshold, one or more corresponding candidate observation poses are adopted as the target observation pose.

[0048] The target observation pose with the highest visual benefit is selected. Under this target observation pose, the collectability of each target mango data is predicted to obtain the corresponding collectability probability. The feature vector of the target observation pose and the target mango data is used to represent the feature vector composed of target depth, fruit stem orientation, local obstacle density, camera observation angle, remaining workspace margin of the robotic arm, risk of branch and leaf crossing, and target stability. If the collectability probability is greater than the collectability threshold, the collectability probability and the target observation pose are recorded in the target mango data to obtain the filtered target mango data; otherwise, it is removed from the current picking batch, and the next candidate observation pose is selected to recalculate the collectability probability until the requirements are met.

[0049] In some more specific embodiments, if the system detects during the view search process that the target is disturbed by strong winds, adjacent fruits sway, branches and leaves bounce back, or local light saturation causes a significant difference between two consecutive observations, the system can trigger a dynamic termination mechanism. This mechanism marks the current target as temporarily off-limits for harvesting and switches to other candidate targets. After completing several other tasks, the robot returns to the target for reassessment. This mechanism helps improve the stability of the overall work cycle.

[0050] In this optional embodiment, in complex tree canopies, although some ripe mangoes are detected, the robotic arm cannot safely approach them. Performing complete inverse kinematics and obstacle avoidance planning for each mango individually would waste a significant amount of time. While individual poses may reveal more information about the fruit stems, they can cause the robotic arm's wrist posture to approach singularities or prevent the end effector from deploying. To address this, the present invention employs an active viewpoint reconstruction strategy, treating local camera / robotic arm movements as perceptual compensation behaviors. When the visibility of the fruit stems in the main viewpoint is insufficient, edge continuity is poor, depth is severely lacking, or the target pose estimation variance is too large, the system automatically generates several local observation poses near the current target. Each observation pose simultaneously considers information gain and execution cost, avoiding significant detours that would otherwise waste time. Traditional methods typically begin reachability analysis only after the perception phase, while the present invention introduces local reachability priors during viewpoint selection.

[0051] Optionally, the viewpoint benefit is expressed as:

[0052] , in, For the j-th candidate observation pose, For the benefits of the aforementioned perspective, For overall visibility of the target, For the fruit stalk to be significant, For the integrity of depth information, For travel time or energy costs, To mitigate the risk of collisions during localized movement, , , , and These are the corresponding perspective benefit weights.

[0053] Specifically, overall target visibility represents the degree of visibility of the target mango's surface. Stem salience is used to measure the prominence of the stem relative to the fruit and background, directly affecting the accuracy of subsequent stem detection and cutting. Depth information integrity is used to measure the completeness of the depth data effectively acquired for the target mango and stem regions. A 3D reconstruction model of the mango is created in the candidate viewing pose (using a dense point cloud or mesh model generated through multi-view stereo vision MVS or point cloud registration), obtaining the camera intrinsic and extrinsic parameters for the current candidate pose. A line-of-sight ray is emitted from the camera optical center of the candidate pose to each vertex or sampling point on the surface of the target mango model. If a ray does not intersect any obstacle before reaching the target mango surface, that surface point is considered visible. The ratio of the number of visible surface points to the total number of surface points on the target mango model is calculated as the overall target visibility. A simulated depth map and normal vector map of the target mango and its surrounding environment are rendered, and the contrast features, edge sharpness, and separation of the stem region are extracted. These features are then weighted and fused to obtain the stem salience score. Acquire a simulated depth map, count the number of effective depth pixels within the bounding box of the target mango, and calculate the standard deviation of the depth values ​​of the effective depth pixels as the integrity of the depth information. Move from the current joint angle configuration to the joint angle configuration corresponding to the candidate pose, and obtain the movement time or energy cost during the process. The collision risk during the local movement process is used to quantify the probability of the robotic arm colliding with the surrounding environment during its movement from the current pose to the candidate pose, and is also used as a negative penalty term. Discretize the collision detection of the robotic arm link model and the environment model during the movement. Calculate the minimum distance between the robotic arm surface and environmental obstacles at each time step on the trajectory, and define the collision risk function as the reciprocal of the minimum distance or an exponential decay function for risk calculation.

[0054] Optionally, the candidate target comprehensive priority function is expressed as: , in, Let i be the priority of the i-th mango. The overall maturity score for the i-th mango is given. Let be the collectability probability of the i-th mango. For visibility score, To cover up punishment, To mitigate the risk of injury, To account for the overall path cost, , , , , and Each of these represents a configurable weight for its corresponding priority level.

[0055] Specifically, the visibility score quantifies the visual observation quality of the i-th mango in the current target viewing pose. Better visibility leads to higher accuracy in subsequent stem detection and cutting localization. The visibility score is obtained by assigning weights to the overall target visibility and stem salience. The occlusion penalty quantifies the complexity of the environment surrounding the i-th mango. More severe occlusion increases the difficulty for the robotic arm to approach and pick the mango, thus incurring a greater penalty. The damage risk quantifies the probability of mechanical damage to the mango itself or surrounding fruits during the picking of the i-th mango. The comprehensive path cost represents the combined path cost resulting from chassis repositioning and robotic arm movement. Lower path costs result in higher picking efficiency.

[0056] In this optional embodiment, a scientifically sound harvesting sequence is generated through a comprehensive evaluation of the above six dimensions, achieving intelligent, efficient, and non-destructive mango harvesting operations. The system establishes a candidate queue according to the priority of mangoes from high to low, and arranges tasks based on target clusters that can be covered by the same stopping position. This reflects both the agricultural logic of harvesting ripe fruits first and the engineering logic of harvesting easy-to-harvest and high-yield fruits first.

[0057] Optionally, the step of constructing a local planning field by modeling the environment using the current priority target and the point cloud data includes: The environment of the current priority target within a preset range is reconstructed using the point cloud data to obtain an obstacle avoidance collision body; The local planning field is constructed using the point cloud data and the obstacle avoidance collision body.

[0058] Specifically, after determining the current priority target, the system uses point cloud data to reconstruct the surfaces of fruits, branches, leaves, and trunks within a certain radius around the current priority target. Based on the three-dimensional coordinates of the current priority target, a local point cloud within a preset range centered on the target is extracted from the global point cloud data. A spherical clipping region is constructed with the centroid of the target mango as the center and a preset radius, and all point cloud data within this region is extracted as the local point cloud. The point cloud belonging to the target mango itself is separated from the local point cloud, and the remaining point cloud is used as the environmental obstacle point cloud. Based on a hybrid hierarchical collision modeling strategy, obstacle avoidance collision bodies in the local environment are reconstructed from the environmental obstacle point cloud. The obstacle avoidance collision body is a geometric model used for collision detection in path planning, and its accuracy and computational efficiency directly affect the planning effect. The hybrid hierarchical collision modeling strategy is used to employ geometric representations of different accuracies for obstacles of different scales. Through point cloud denoising, smoothing, clustering, and bounding volume generation, fruit collision bodies, branch and slender stem collision bodies, leaf soft obstacle bodies, and robotic arm restricted areas are constructed respectively, providing environmental constraints for subsequent path planning. A local planning field is constructed using local point cloud data and an obstacle avoidance collision object set. The local planning field is a potential field model used to guide the robotic arm's end effector smoothly from its current position to the target picking posture; it is composed of a superposition of gravitational and repulsive fields. The gravitational field attracts the robotic arm's end effector towards the target picking posture. The repulsive field repels the robotic arm's end effector away from the obstacle avoidance collision object, thus avoiding collisions. For deformable leaves, the system can assign a lower cost rather than absolute impassability to improve planning flexibility; for main branches and fruit, a higher cost or hard constraint is assigned to avoid collisions.

[0059] Optionally, the step of performing path planning through the local planning field to obtain an executable trajectory includes: Multiple feasible coarse paths are generated through the local planning field; The feasible coarse-grained paths are optimized using a trajectory cost function to obtain the executable trajectory; The trajectory cost function is expressed as: , in, For the cost of trajectory, This is the feasible coarse path. For the trajectory length, The cost of colliding with obstacles, Due to energy consumption and the cost of joint movement, As a cost to trajectory smoothness, Due to the time cost of execution, , , , and These are the corresponding trajectory cost weights.

[0060] Specifically, a local planning field is established based on the point cloud and colliders. Using this constructed local planning field, multiple feasible coarse paths are generated from the current pose of the robotic arm's end effector towards the target picking pose. This is combined with the Rapidly-exploring Random Tree Star (RRT) algorithm. A fast search tree is used to generate multiple feasible coarse-grained paths. During the tree generation process, parent nodes are continuously rewired and locally optimized. The trajectory cost function is used to evaluate all feasible coarse-grained paths to obtain the trajectory cost. All trajectory costs are sorted, and the path with the lowest cost is selected as the executable trajectory.

[0061] In this optional embodiment, multiple feasible coarse paths are generated through a local planning field, and trajectory optimization is performed using a trajectory cost function to obtain an executable trajectory. This effectively overcomes the defect of traditional single planning methods that are prone to getting trapped in local minima. The trajectory cost function comprehensively considers costs from multiple dimensions and selects the trajectory with the best overall performance from multiple paths, achieving a task-level balance between safety, speed, and energy consumption.

[0062] Optionally, it also includes a damage risk assessment module, which is used to monitor the picking process in real time using the actuator output module to obtain mango picking monitoring data; and to obtain a clamping damage risk index through the mango picking monitoring data, wherein the clamping damage risk index is used to correct the picking control command.

[0063] Specifically, in order to control the risk of crush injuries, this invention defines a clamping damage risk index. This can be achieved by comprehensively considering factors such as peak contact pressure, pressure distribution variance, contact duration, estimated fruit hardness, and acceleration. The clamping damage risk index is expressed as follows: , in, For maximum contact pressure, For contact pressure Distribution variance For the duration of clamping, To accelerate the fruit, To effectively cover the area, , , , and These are the weights for each element of the clamping damage risk. (Based on...) With online monitoring, the system can proactively reduce the clamping force, change the approach angle, or abandon the current grip when the risk of damage is too high.

[0064] In some more specific embodiments, the mango harvesting robot system also includes an anomaly diagnosis and self-recovery module. This module receives feedback from vision, force, electrical current, joint status, and the end-effector micro-camera to identify anomalies such as misalignment, empty grasping, slippage, failed shearing, excessive fruit swaying, branch obstruction, and chassis posture changes. It then implements closed-loop handling through an action template library and a recovery strategy library. The system also includes an edge-cloud collaboration and digital twin platform to record tree structure, single-tree target distribution, harvesting trajectory, fault statistics, operation time, damage events, and model version information. The edge device can operate independently even when offline or in a weak network environment. After network recovery, it synchronizes key logs to the cloud for subsequent model iterations and equipment maintenance.

[0065] In this optional embodiment, closed-loop safety monitoring of the harvesting process is achieved, effectively avoiding fruit damage problems caused by fixed force control parameters or environmental disturbances in traditional open-loop control. By collecting harvesting process data in real time, the damage risk under the current clamping state can be dynamically assessed. When the risk index exceeds the safety threshold, the control command is corrected in time to reduce the clamping force or adjust the gripping posture, fundamentally ensuring the integrity of the mango fruit and significantly reducing the mechanical damage rate during the harvesting process.

[0066] like Figure 2 As shown in the figure, an embodiment of the present invention provides a mango harvesting robot method for high-branch, densely shrubbed orchards, applied to the aforementioned mango harvesting robot system for high-branch, densely shrubbed orchards. The mango harvesting robot method includes: Step 110: Perform timestamp calibration and sensor synchronization processing on the original multi-sensor data to obtain the processed original multi-sensor data, wherein the original sensor data includes image data and point cloud data; Step 120: Based on a lightweight target detection network, perform joint detection of mango fruit body and stem on the image data to obtain multiple target mango data; Step 130: Active perspective reconstruction and joint decision-making screening of the target mango data are performed to obtain the screened target mango data; the candidate target comprehensive priority function is used to prioritize the screened target mango data to generate a target picking sequence. Step 140: Obtain the current priority target based on the target picking sequence; perform environmental modeling using the current priority target and the point cloud data to construct a local planning field; perform path planning using the local planning field to obtain an executable trajectory; Step 150: Based on the pose-force collaborative control algorithm, the executable trajectory is converted into picking control commands, wherein the picking control commands are used to control the robotic arm and end effector to perform cutting.

[0067] In some more specific embodiments, if the fruit stem is not visible or the positioning variance is too large, the active view module generates a supplementary view pose and performs view optimization; after confirming that the target is collectable, the local path planning module generates an obstacle avoidance approach trajectory; the robotic arm is controlled to approach the target, and then the end effector is controlled to complete compliant wrapping, fruit stem guidance and directional stem cutting; if the anomaly diagnosis module detects clamping deviation, empty grasping or stem cutting not broken, the corresponding recovery action is performed.

[0068] The mango harvesting robot method for high-branch, densely irrigated orchards in this embodiment is used to implement the mango harvesting robot system for high-branch, densely irrigated orchards as described above. Its advantages over the prior art are the same as those of the mango harvesting robot system for high-branch, densely irrigated orchards as described above, and will not be repeated here.

[0069] While the present invention has been disclosed above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention, and all such changes and modifications will fall within the scope of protection of the present invention.

Claims

1. A mango harvesting robot system for high-branch, densely irrigated orchards, characterized in that, include: The sensor data acquisition module is used to perform timestamp calibration and sensor synchronization processing on the raw multi-sensor data to obtain the processed raw multi-sensor data, wherein the processed raw sensor data includes image data and point cloud data. The multimodal perception inference module is used to perform joint detection of mango fruit body and stem on the image data based on a lightweight target detection network to obtain multiple target mango data. The active perspective reconstruction module is used to perform active perspective reconstruction and joint decision-making screening of the target mango data to obtain the screened target mango data; the candidate target comprehensive priority function is used to prioritize the screened target mango data to generate a target picking sequence. The task planning and scheduling module is used to obtain the current priority target based on the target picking sequence; to perform environmental modeling using the current priority target and the point cloud data to construct a local planning field; and to perform path planning using the local planning field to obtain an executable trajectory. The actuator output module is used to convert the executable trajectory into picking control commands based on the pose-force collaborative control algorithm, wherein the picking control commands are used to control the robotic arm and the end effector to perform cutting.

2. The mango harvesting robot system for high-branch, densely irrigated orchards according to claim 1, characterized in that, The target mango data includes mature mango detection data and the corresponding area where the fruit stem connects to the fruit body. The image data is then subjected to joint detection of the mango fruit body and fruit stem using a lightweight target detection network, resulting in multiple target mango data sets, including: Target detection is performed on the image data to obtain multiple candidate mango detection data; The maturity of each candidate mango was comprehensively evaluated to obtain a comprehensive maturity score. The candidate mango detection data are filtered based on the comprehensive maturity score to obtain the mature mango detection data; When the confidence level of the fruit stem region in the mature mango detection data is higher than a preset value, the candidate mango detection data is input into the instance segmentation network to obtain a fine-grained fruit stem mask. The fine-grained fruit stalk mask is refined, noise filtered, and skeletalized to obtain the connection area between the fruit stalk and the fruit body.

3. The mango harvesting robot system for high-branch, densely irrigated orchards according to claim 2, characterized in that, The overall maturity score is expressed as follows: ， in, The overall maturity score for the i-th mango is given. Let i be the color ripeness feature component of the i-th mango. Let be the texture maturity feature component of the i-th mango. Let be the near-infrared or near-infrared surrogate feature component of the i-th mango. For the i-th mango, the historical time-series consistency component is... , , and These are the maturity assessment weighting coefficients, and .

4. The mango harvesting robot system for high-branch, densely irrigated orchards according to claim 3, characterized in that, The process of actively reconstructing and jointly selecting the usability of each target mango data to obtain the selected target mango data includes: The target mango data are observed from a main perspective to generate multiple candidate observation poses. Each candidate observation pose is evaluated for viewpoint gain to obtain viewpoint gain. The candidate observation poses are filtered using the aforementioned viewpoint gain to obtain the target observation pose; The collectability of each target mango data is predicted by the target observation pose to obtain the corresponding collectability probability. The collectability probability is expressed as: ， in, Let be the collectability probability of the i-th mango. For the Sigmoid function, The feature vectors of the target observation pose and the target mango data. and These are the parameters of the feasible probability model; The target mango data is filtered using the available probability to obtain the filtered target mango data.

5. The mango harvesting robot system for high-branch, densely irrigated orchards according to claim 4, characterized in that, The benefit from that perspective is expressed as: ， in, For the j-th candidate observation pose, For the benefits of the aforementioned perspective, For overall visibility of the target, For the fruit stalk to be significant, For the integrity of depth information, For travel time or energy costs, To mitigate the risk of collisions during localized movement, , , , and These are the corresponding perspective benefit weights.

6. The mango harvesting robot system for high-branch, densely irrigated orchards according to claim 4, characterized in that, The comprehensive priority function for the candidate targets is expressed as follows: ， in, Let i be the priority of the i-th mango. The overall maturity score for the i-th mango is given. Let be the collectability probability of the i-th mango. For visibility score, To cover up punishment, To mitigate the risk of damage, To account for the overall path cost, , , , , and Each of these represents a configurable weight for its corresponding priority level.

7. The mango harvesting robot system for high-branch, densely irrigated orchards according to claim 1, characterized in that, The step of modeling the environment using the current priority target and the point cloud data to construct a local planning field includes: The environment of the current priority target within a preset range is reconstructed using the point cloud data to obtain an obstacle avoidance collision body; The local planning field is constructed using the point cloud data and the obstacle avoidance collision body.

8. The mango harvesting robot system for high-branch, densely irrigated orchards according to claim 1, characterized in that, The step of obtaining an executable trajectory through path planning using the local planning field includes: Multiple feasible coarse paths are generated through the local planning field; The feasible coarse-grained paths are optimized using a trajectory cost function to obtain the executable trajectory; The trajectory cost function is expressed as: ， in, For the cost of trajectory, This is the feasible coarse path. For the trajectory length, The cost of colliding with obstacles, Due to energy consumption and the cost of joint movement, As a cost to trajectory smoothness, Due to the time cost of execution, , , , and These are the corresponding trajectory cost weights.

9. The mango harvesting robot system for high-branch, densely irrigated orchards according to claim 1, characterized in that, It also includes a damage risk assessment module, which is used to monitor the picking process in real time using the actuator output module to obtain mango picking monitoring data; and to obtain a clamping damage risk index from the mango picking monitoring data, wherein the clamping damage risk index is used to correct the picking control command.

10. A method for using a mango harvesting robot in a densely irrigated orchard with tall branches, characterized in that, The mango harvesting robot system, applicable to any one of claims 1 to 9, for orchards with high branches and dense shrubs, comprises: The original multi-sensor data is time-stamped and synchronized with the sensors to obtain the processed original multi-sensor data, wherein the original sensor data includes image data and point cloud data. Based on a lightweight target detection network, the image data is used to perform joint detection of mango fruit body and stem to obtain multiple target mango data. Active perspective reconstruction and joint decision-making screening of the target mango data are performed to obtain the screened target mango data; the candidate target comprehensive priority function is used to prioritize the screened target mango data to generate a target picking sequence. The current priority target is obtained based on the target picking sequence; environmental modeling is performed using the current priority target and the point cloud data to construct a local planning field; path planning is performed using the local planning field to obtain an executable trajectory; Based on the pose-force collaborative control algorithm, the executable trajectory is converted into picking control commands, wherein the picking control commands are used to control the robotic arm and end effector to perform cutting.

Images