A citrus picking robot motion control method based on occlusion estimation

Abstract

The present application relates to the technical field of intelligent control, in particular to a citrus picking robot motion control method based on occlusion estimation.The present application controls the mechanical arm to hover and applies a pneumatic pulse, and combines electronic image stabilization to collect the timing image stream of the occlusion area; the main vibration, amplitude and damping attenuation rate are extracted by superpixel time-frequency analysis, the environment is decoupled and classified into soft occlusion, fragile hard constraint and rigid hard constraint area, and a probability grid map is constructed; an anisotropic low impedance channel along the texture direction is constructed for the soft occlusion area, and a high impedance repulsion field is constructed for the hard constraint area; the controller dynamically adjusts the stiffness and damping parameters in the Cartesian space according to the real-time position of the mechanical arm.The present application realizes non-contact environmental attribute perception, realizes compliant picking through the leaves while ensuring safe obstacle avoidance, and significantly improves the work efficiency in unstructured environment.

Description

Technical Field

[0001] This invention relates to the field of intelligent control technology, specifically to a motion control method for a citrus harvesting robot based on occlusion estimation. Background Technology

[0002] With the development of modern agricultural technology, automated harvesting of citrus and other fruits has become an important way to solve the shortage of agricultural labor and reduce production costs. Citrus orchards are complex, unstructured environments where fruits often grow in areas with dense foliage and are partially obscured by branches, leaves, or other fruits, posing a significant challenge to the motion control of harvesting robots.

[0003] Existing motion control methods for harvesting robots mainly fall into two categories. One category is based on traditional obstacle avoidance path planning methods, which typically treat all obstructions (whether fragile leaves or hard branches) identified by visual sensors as insurmountable rigid obstacles. When planning a path, the robot tends to seek a collision-free path that completely avoids all obstacles; this strategy often leads to path planning failure in dense citrus canopies or results in a circuitous and lengthy path, severely reducing harvesting efficiency. The other category attempts to introduce active interaction strategies to address the obstruction problem. By controlling the robotic arm to actively approach and contact obstacles, and using tactile sensors to obtain contact force feedback, the robot constructs the state space of the obstacle, thereby determining the obstacle's properties and implementing interventional harvesting. However, this detection method, which relies on tentative contact, has significant drawbacks: First, it can only obtain the physical properties of the obstruction after the robotic arm physically interacts with the environment, which not only increases the time cost of the control process and reduces the work cycle, but also makes it easy for frequent tentative contacts to damage the robotic arm's end effector, cause fruit to fall accidentally, or cause irreversible mechanical damage to the branches and trunks of the fruit tree.

[0004] In practical operations, obstacles exhibit significant differences in type. For example, soft leaves allow the robotic arm to pass directly, while thick branches must be avoided. While current vision systems can locate obstacles, they struggle to effectively estimate the flexibility or type of obstacles based solely on visual data before the motion planning stage. Lacking this prior "occlusion estimation," the control system cannot intelligently generate matching strategies in the early planning stages—that is, generating "crossing" paths for soft occlusions and "detour" paths for hard occlusions. This perceptual lag and the limited range of strategies cause existing harvesting robots to either be overly conservative and sacrifice efficiency, or overly aggressive and risk collisions when facing complex occlusion environments, making it difficult to obtain the optimal harvesting path.

[0005] In summary, how to estimate the type and flexibility of occlusions under non-contact conditions and adaptively generate robot motion control strategies is a technical challenge that urgently needs to be solved.

[0006] To address this, a motion control method for citrus harvesting robots based on occlusion estimation is proposed. Summary of the Invention

[0007] The purpose of this invention is to provide a motion control method for a citrus harvesting robot based on occlusion estimation. This invention controls the robotic arm to hover and applies pneumatic pulses, combined with electronic image stabilization to acquire a time-series image stream of the occluded area; it uses superpixel time-frequency analysis to extract the dominant vibration, amplitude, and damping attenuation rate, decoupling and classifying the environment into soft occlusion, vulnerable hard constraint, and rigid hard constraint regions, and constructs a probabilistic grid map; for soft occlusion regions, it constructs an anisotropic low-impedance channel along the texture direction, and for hard constraint regions, it constructs a high-impedance repulsion field; the controller dynamically adjusts the Cartesian space stiffness and damping parameters according to the real-time position of the robotic arm. This invention achieves non-contact environmental attribute perception, enabling compliant harvesting through leaves while ensuring safe obstacle avoidance, significantly improving work efficiency in unstructured environments.

[0008] To achieve the above objectives, the present invention provides the following technical solution:

[0009] A motion control method for a citrus harvesting robot based on occlusion estimation includes:

[0010] Control the robotic arm to hover in front of the occluded area and apply pneumatic pulses; acquire a time-series image stream of the occluded area;

[0011] The image is segmented into superpixel regions, and the displacement signal of each superpixel region is analyzed in time and frequency to extract the principal vibration, amplitude, and damping attenuation rate. Based on the time and frequency characteristics, the region is decoupled and classified into soft occlusion regions, vulnerable hard constraint regions, and rigid hard constraint regions, and a semantic map is generated.

[0012] A probabilistic raster map is constructed based on semantic graphs, and the regional attribute probabilities of each raster are updated using Bayesian filters. For soft occlusion regions, low-impedance channels that allow passage are constructed according to texture direction, and the desired impedance parameters for anisotropy are set. For vulnerable hard-constrained regions and rigid hard-constrained regions, high-impedance virtual repulsion fields are constructed, and the parameters for high-impedance repulsion fields are set. A virtual impedance map is generated based on the low-impedance channels and the high-impedance repulsion fields.

[0013] Based on the real-time position of the robotic arm's end effector in the virtual impedance map, the controller adjusts the Cartesian space stiffness and damping parameters of the robotic arm using the desired impedance parameter and the high impedance repulsion field parameter.

[0014] Preferably, the controller adaptively adjusts the output power of the pneumatic excitation device in the following ways: setting the desired impact pressure threshold of the target obstruction surface; the controller reads the depth distance of the obstruction center point fed back by the depth vision sensor in real time; a nonlinear mapping relationship between output power and depth distance is constructed, specifically determined through pre-experimental calibration, establishing the correspondence between the outlet pressure of the pneumatic excitation device and the depth distance, so that after the airflow diffuses in the air and loses kinetic energy, the airflow impact force reaching the surface of the obstruction at different depths remains within the preset effective excitation range; wherein, the change trend of the outlet pressure is a nonlinear relationship that increases sharply with the increase of depth distance, so as to offset the rapid attenuation of the airflow impact force in free space with distance.

[0015] Preferably, the acquisition of a time-series image stream of the occluded area further includes electronic image stabilization processing of the image stream using inertial measurement unit data, and: acquiring six-degree-of-freedom jitter data of the robotic arm end effector acquired by the inertial measurement unit, calculating the instantaneous displacement vector of the camera optical center through inverse kinematics calculation; and using the instantaneous displacement vector to perform pixel-level inverse compensation on the acquired time-series image frames to eliminate the global background displacement caused by the jitter of the robotic arm hovering servo.

[0016] The specific steps of superpixel segmentation include: processing the first frame image after image stabilization using a simple linear iterative clustering algorithm to aggregate pixels with similar color and spatial distance into perceptually meaningful superpixel blocks; using the geometric center of the superpixel blocks as the tracking feature point for optical flow tracing in subsequent frames; performing non-stationary signal analysis on the displacement time-series signal obtained from the tracking using the Hilbert-Huang transform algorithm, specifically including using empirical mode decomposition to decompose the displacement signal into several intrinsic mode function components, and performing Hilbert spectrum analysis on each component to extract instantaneous frequency and instantaneous energy distribution; extracting the maximum amplitude feature based on the maximum value of the instantaneous energy distribution; identifying the initial vibration response at the moment of airflow contact based on the jump characteristics of the instantaneous frequency, and calculating the damping attenuation rate based on the energy attenuation characteristics of the Hilbert spectrum.

[0017] Preferably, the specific method for decoupling and classifying occluders based on time-frequency characteristics to generate a semantic map is as follows: preset frequency threshold, amplitude threshold, and attenuation rate threshold; if the maximum amplitude of a superpixel block under airflow excitation is lower than the amplitude threshold, it is determined to be a robust branch with high structural stiffness and marked as a rigid hard constraint region; if the maximum amplitude of a superpixel block is higher than the amplitude threshold, and the damping attenuation rate after the airflow stops is lower than the attenuation rate threshold, it is determined to be a fruiting branch and / or fruit with high inertia and pendulum effect and marked as a vulnerable hard constraint region; if the maximum amplitude of a superpixel block is higher than the amplitude threshold, and the damping attenuation rate after the airflow stops is higher than the attenuation rate threshold, it is determined to be a leaf and marked as a soft occlusion region.

[0018] Preferably, the specific process of constructing a probabilistic grid map based on a semantic graph includes: constructing a three-dimensional voxel grid map and initializing the prior probability log ratio of each grid; using the intrinsic parameter matrix of the depth vision sensor and the pose of the robotic arm's end effector, back-projecting the geometric center of each superpixel in the semantic graph, combined with its depth value, to the world coordinate system; using a ray casting model, marking the grid where the projection point is located as an occupied observation of the corresponding semantic category, and marking the grids on the path from the camera's optical center to the projection point as free-space observations; using a Bayesian filtering algorithm, taking the current occupied observations and free-space observations as input, recursively updating the posterior probability of each grid belonging to soft occlusion, vulnerable hard constraints, and rigid hard constraints. Log-likelihood ratio; a fully connected conditional random field algorithm is introduced to globally optimize the 3D voxel map. By minimizing the energy function, which includes a univariate potential function based on posterior probability and a binary potential function based on color and spatial distance between voxels, spatial smoothing constraints are applied to the semantic labels of adjacent voxels, filtering out discrete noise points and filling semantic gaps caused by occlusion. When the posterior probability of a hard-constrained grid exceeds a safety threshold, the grid is subjected to 3D morphological dilation when generating a virtual impedance map to construct a safety buffer. The free space observation corresponds to a preset negative log-likelihood ratio in the Bayesian filtering update, and the log-likelihood ratio of the posterior probability of a grid belonging to any type of obstacle on the negative update path.

[0019] Preferably, the specific method for constructing a low-impedance channel that allows passage through soft occlusion areas includes: extracting texture features from superpixel blocks identified as soft occlusion areas, and calculating the main growth direction of the leaf texture using the image gradient direction histogram; constructing a three-dimensional impedance ellipsoid model, setting the major axis of the impedance ellipsoid to be parallel to the main growth direction of the leaf texture, and setting the minor axis of the impedance ellipsoid to be perpendicular to the main growth direction of the leaf texture; setting a first translational stiffness coefficient in the major axis direction to allow the robotic arm to generate compliant displacement along the leaf growth texture direction; and setting a second translational stiffness coefficient in the minor axis direction to limit the extrusion of the robotic arm perpendicular to the leaf texture direction.

[0020] The specific method for establishing a high-impedance repulsion field for grids identified as hard-constrained regions includes: constructing an artificial potential field repulsion model with the grid center identified as a vulnerable hard-constrained region or a rigid hard-constrained region as the potential field source point; for the rigid hard-constrained region, setting an isotropic first repulsion stiffness coefficient and a first repulsion radius to form a strong repulsion boundary; for the vulnerable hard-constrained region, setting an isotropic second repulsion stiffness coefficient and a second repulsion radius to form a protective repulsion field with a larger safety buffer; the first repulsion stiffness coefficient is greater than the second repulsion stiffness coefficient, and the second repulsion radius is greater than the first repulsion radius.

[0021] Preferably, adjusting the Cartesian stiffness and damping parameters of the robotic arm specifically includes: when the robotic arm is in a soft-obstruction area, setting the translational stiffness coefficient in the Cartesian stiffness matrix to a first preset stiffness value, and when the robotic arm is subjected to the contact force of the blade, the actual position relative to the desired position generates a compliant displacement, which absorbs the collision energy and slides along the blade surface; when the robotic arm approaches a hard-constrained area, setting the translational stiffness coefficient in the Cartesian stiffness matrix to a second preset stiffness value, and superimposing a virtual repulsive force based on the gradient of the artificial potential field, the position deviation pointing towards the hard-constrained area generates a reverse correction force in the controller to overcome the inertia of the robotic arm and external interference;

[0022] In the virtual impedance map, a stiffness transition zone of preset thickness is defined at the interface between the soft obstruction region and free space. Within the stiffness transition zone, the distance from the end of the robotic arm to the boundary of the soft obstruction region is used as the independent variable, and a stiffness change curve is constructed using an S-curve function. During the process of the end of the robotic arm entering the soft obstruction region from free space, the Cartesian space stiffness parameter is controlled to continuously and monotonically decrease along the S-curve function to a preset crossing stiffness value.

[0023] Preferably, achieving compliant passage through soft obstruction areas also includes setting a joint torque saturation threshold. The specific logic is as follows: during the process of the robotic arm passing through the soft obstruction area, the current value of each joint motor is monitored in real time, and the external contact torque, including the leaf contact torque and the branch destruction torque, is estimated; a floating threshold is set, which is higher than the leaf contact torque but lower than the branch destruction torque; when the rate of change of the monitored external contact torque exceeds the floating threshold, it is determined that an invisible hard obstacle has been touched, and the controller immediately switches the robotic arm to gravity compensation mode and marks the obstacle position as a new hard constraint point to be updated in the semantic graph.

[0024] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0025] 1. This invention employs a combination of hovering active aerodynamic excitation and time-frequency analysis. By analyzing the vibration modes of obstructions under airflow, it can accurately distinguish between soft obstructions (leaves) with high damping characteristics, vulnerable hard constraints (fruits) with pendulum effects, and rigid hard constraints (branches). This non-contact sensing method allows the robot to obtain the physical properties of the environment before a physical collision occurs, fundamentally eliminating the risk of fruit falling or branches being damaged due to blind exploration. Simultaneously, electronic image stabilization and distance-adaptive pressure adjustment ensure the robustness and accuracy of the sensing data.

[0026] 2. This invention overcomes the limitation of traditional path planning that treats obstacles as isotropic rigid bodies, proposing an anisotropic impedance modeling method based on texture direction. By identifying the texture growth direction of soft occlusion areas, a low-impedance channel along the texture direction is established, allowing the robotic arm to compliantly traverse like "gliding" over a leaf, while maintaining high impedance in the direction perpendicular to the texture to prevent excessive compression. This strategy avoids a one-size-fits-all "detour" approach to all visual obstructions, greatly shortening the robotic arm's movement path length and reducing redundant obstacle avoidance actions, thereby significantly improving the efficiency and cycle time of harvesting operations in complex unstructured citrus environments.

[0027] 3. This invention employs a variable stiffness compliant control strategy, dynamically adjusting the Cartesian space stiffness based on the robot arm's real-time position on a virtual impedance map. Stiffness is reduced when traversing soft obstruction zones to adapt to environmental forces, and increased when approaching hard constraint zones to force obstacle avoidance. This invention also incorporates a torque saturation threshold protection mechanism that dynamically changes with the theoretical contact torque. For "invisible hard obstacles" (tree branches completely obscured by leaves) that are not visually detectable, it can detect abnormal torque mutations at the moment of accidental rigid contact by the robot arm and immediately trigger gravity compensation or retraction protection. This combined hardware and software proactive safety strategy significantly improves the robot's operational robustness in high-uncertainty environments and the system's self-protection capabilities. Attached Figure Description

[0028] Figure 1 A flowchart of a motion control method for a citrus picking robot based on occlusion estimation provided in an embodiment of the present invention;

[0029] Figure 2 This is a schematic diagram of the occlusion classification decision based on time-frequency features and vibration ellipsoid provided in an embodiment of the present invention;

[0030] Figure 3 This is a schematic diagram of the robot's Cartesian space stiffness mode switching state provided in an embodiment of the present invention. Detailed Implementation

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

[0032] Please see Figures 1 to 3 This invention provides a motion control method for a citrus harvesting robot based on occlusion estimation, the technical solution of which is as follows:

[0033] A motion control method for a citrus harvesting robot based on occlusion estimation includes:

[0034] Control the robotic arm to hover in front of the occluded area and apply pneumatic pulses; acquire a time-series image stream of the occluded area;

[0035] The image is segmented into superpixel regions, and the displacement signal of each superpixel region is analyzed in time and frequency to extract the principal vibration, amplitude, and damping attenuation rate. Based on the time and frequency characteristics, the region is decoupled and classified into soft occlusion regions, vulnerable hard constraint regions, and rigid hard constraint regions, and a semantic map is generated.

[0036] A probabilistic raster map is constructed based on semantic graphs, and the regional attribute probabilities of each raster are updated using Bayesian filters. For soft occlusion regions, low-impedance channels that allow passage are constructed according to texture direction, and the desired impedance parameters for anisotropy are set. For vulnerable hard-constrained regions and rigid hard-constrained regions, high-impedance virtual repulsion fields are constructed, and the parameters for high-impedance repulsion fields are set. A virtual impedance map is generated based on the low-impedance channels and the high-impedance repulsion fields.

[0037] Based on the real-time position of the robotic arm's end effector in the virtual impedance map, the controller adjusts the Cartesian space stiffness and damping parameters of the robotic arm using the desired impedance parameter and the high impedance repulsion field parameter.

[0038] Example 1:

[0039] This embodiment is applied to a navel orange orchard with high canopy closure. In this scenario, mature fruit (the target object) is often located at a depth of about 30cm-50cm inside the canopy, covered by 2-3 layers of staggered leaves (soft shading), and randomly distributed among the leaves are dead branches with a diameter of 0.5cm-2cm (rigid constraints) and adjacent immature green fruit (vulnerable constraints). Traditional visual servo robotic arms, unable to penetrate the leaves to obtain depth information, often consider this area unreachable or prone to rigid collisions.

[0040] As one embodiment of the present invention, refer to Figure 1 A flowchart of a motion control method for a citrus harvesting robot based on occlusion estimation, referring to... Figure 2 A schematic diagram of occlusion classification decision based on time-frequency characteristics and vibration ellipsoid, referring to... Figure 3 A schematic diagram of the robot's Cartesian space stiffness mode switching state.

[0041] Furthermore, the controller adaptively adjusts the output power of the pneumatic excitation device in the following ways: setting the desired impact pressure threshold of the target obstruction surface; the controller reads the depth distance of the obstruction center point fed back by the depth vision sensor in real time; and constructing a nonlinear mapping relationship between output power and depth distance. Specifically, this is determined through pre-experimental calibration, establishing a correspondence between the outlet pressure of the pneumatic excitation device and the depth distance, so that after the airflow diffuses and loses kinetic energy in the air, the airflow impact force reaching the obstruction surface at different depths remains within the preset effective excitation range. Among these, the change trend of the outlet pressure is a nonlinear relationship that increases sharply with the increase of depth distance, in order to offset the rapid attenuation of the airflow impact force in free space with distance.

[0042] The specific aerodynamic excitation parameters are set as follows: the pulse duration for each aerodynamic excitation is set between 0.3s and 0.8s, preferably 0.5s. A single-order square wave output is used, and the airflow is immediately cut off at the end to capture the free decay response. The nonlinear mapping relationship between output power and depth distance is obtained through pre-calibration within a depth range of 200mm to 800mm at 100mm sampling intervals. Based on the calibration data, a quadratic polynomial fitting is used to construct the mapping model. That is, the outlet pressure is set as a quadratic function of the depth distance of the center point of the obstruction. This function includes a quadratic term proportional to the square of the distance, a linear term proportional to the distance, and a constant term. The coefficients are determined by least squares fitting to ensure that the energy attenuation of the airflow during free space propagation can be offset, so that the impact pressure reaching the target is maintained within the effective excitation range.

[0043] Specifically, wind pressure was pre-calibrated for the navel orange orchard environment, and the effective excitation range of the target shading surface was set to 150Pa to 250Pa. This pressure range is sufficient to drive the leaves to produce an observable displacement of 3mm to 5mm, while the resulting impact force is far below the shear force threshold of about 15N for the mature navel orange stalk. The controller stores a table of discrete control parameters based on experimental measurements. When the depth vision sensor detects an obstruction in the near field 300mm in front of the robotic arm's end effector, the controller sends a command to the electro-proportional valve via the D / A conversion module to adjust the nozzle outlet pressure of the pneumatic excitation device to 0.15MPa. At this point, the residual pressure of the airflow reaching the blade surface after air damping is approximately 180Pa, which is within the safe excitation range. When the sensor reports that the obstruction is located in a 600mm deep region, although the distance only doubles, in order to overcome the impact force attenuation caused by airflow diffusion and kinetic energy loss in free space, the controller significantly increases the outlet pressure to 0.45MPa based on a pre-calibrated nonlinear mapping relationship, rather than a simple linear doubling. This ensures that the airflow, after long-distance transmission, can still maintain an impact pressure of approximately 170Pa when it reaches the surface of the deep branches.

[0044] This invention solves the problem that a single air pressure cannot adapt to complex canopy depth variations by establishing a nonlinear mapping relationship between output power and depth. When airflow propagates through the air, its kinetic energy rapidly decays. If the air pressure is constant, distant leaves cannot be effectively moved, leading to missed detections, while nearby fruits may be blown off by strong airflow. This solution ensures that the impact force of the airflow reaching the surface of obstructions at different depths remains within the optimal window of "moving leaves but not blowing off fruits," greatly improving the robustness of the sensing system and effectively preventing agricultural economic losses caused by the detection method itself.

[0045] Before segmenting the image into superpixel regions, the Euler video upscaling algorithm is applied as a preprocessing step to the acquired temporal image stream of the occluded region. Specifically, this includes: performing Laplacian pyramid spatial decomposition and temporal bandpass filtering on the temporal image stream to separate specific frequency band signals corresponding to the inherent frequency range of the rigid hard constraint region; linearly amplifying the pixel color changes or phase changes within the specific frequency band; and superimposing the amplified signals back into the original image to visually significantly enhance the amplitude of the minute physical vibrations of rigid branches and deep occlusions.

[0046] This invention, by introducing Euler video magnification technology, essentially endows the robot's vision system with "microscopic" capabilities, solving the technical challenge of capturing the extremely small (sub-pixel level) physical displacement of thick branches or deep targets under long-distance or weak airflow excitation, which is difficult for optical flow algorithms to detect. This method significantly improves the signal-to-noise ratio of visual perception, enabling the robot to clearly distinguish between "trembling" rigid obstacles and "stationary" backgrounds without relying on high-power airflow. While reducing aerodynamic energy consumption, it effectively prevents the risk of collisions caused by missing deep hard obstacles.

[0047] Furthermore, acquiring the temporal image stream of the occluded area also includes electronic image stabilization processing of the image stream using inertial measurement unit data, and: acquiring the six-degree-of-freedom jitter data of the robotic arm end effector acquired by the inertial measurement unit, calculating the instantaneous displacement vector of the camera optical center through inverse kinematics calculation; and using the instantaneous displacement vector to perform pixel-level inverse compensation on the acquired temporal image frames to eliminate the global background displacement caused by the jitter of the robotic arm hovering servo.

[0048] The specific steps of superpixel segmentation include: processing the first frame image after image stabilization using a simple linear iterative clustering algorithm to aggregate pixels with similar color and spatial distance into perceptually meaningful superpixel blocks; using the geometric center of the superpixel blocks as the tracking feature point for optical flow tracing in subsequent frames; performing non-stationary signal analysis on the displacement time-series signal obtained from the tracking using the Hilbert-Huang transform algorithm, specifically including using empirical mode decomposition to decompose the displacement signal into several intrinsic mode function components, and performing Hilbert spectrum analysis on each component to extract instantaneous frequency and instantaneous energy distribution; extracting the maximum amplitude feature based on the maximum value of the instantaneous energy distribution; identifying the initial vibration response at the moment of airflow contact based on the jump characteristics of the instantaneous frequency, and calculating the damping attenuation rate based on the energy attenuation characteristics of the Hilbert spectrum.

[0049] The specific calculation process includes: First, time integration is performed on the Hilbert spectrum of the displacement signal after the airflow pulse stops to obtain the decay curve of vibration energy over time; second, a function model containing an exponential decay term is used to fit the energy decay curve. This function model consists of the initial energy amplitude multiplied by a term with a base of the natural constant and an exponent of the product of the negative damping decay rate and time, plus the background noise energy constant; finally, the coefficient of the exponential term in the fitted function is extracted as the damping decay rate, which directly reflects the rate at which the vibration energy of the obstruction is dissipated.

[0050] After decomposing the displacement signal into several intrinsic mode function components using empirical mode decomposition (EMD), these components need to be screened to remove noise and redundant components. The specific screening principles are: first, exclude high-frequency noise components, i.e., exclude intrinsic mode functions with a dominant frequency higher than 100Hz; second, exclude trend components, i.e., exclude residual components and intrinsic mode functions with frequencies close to zero. Only the intrinsic mode function components containing the main vibration information are retained for Hilbert spectral analysis to ensure that the extracted instantaneous frequency and instantaneous energy distribution characteristics accurately reflect the inherent physical properties of the obstruction.

[0051] Specifically, when a six-DOF robotic arm is fully extended to 1.5 meters for deep canopy work, the flexible cantilever structure and the micro-motion characteristics of the servo motor when locked result in high-frequency physical vibrations at the end of the arm, typically ranging from 10Hz to 15Hz and 0.5mm to 1.5mm in amplitude. If left unprocessed, these minute mechanical vibrations, when projected onto a 1280×720 resolution visual image, would cause a global background drift of 3 to 6 pixels per frame, easily leading the algorithm to misjudge a stationary, thick tree trunk as a vibrating target. By using inertial measurement unit data sampled at 200Hz, the minute displacement vector of the camera's optical center at each exposure moment is calculated, and reverse translation compensation is applied to the image, ensuring the background appears absolutely still visually.

[0052] Subsequently, in the superpixel processing stage, the original high-resolution image containing nearly one million pixels is aggregated into approximately 600 to 800 superpixel blocks with similar color textures. For example, a single citrus leaf is aggregated into a green superpixel block containing approximately 1200 pixels. The optical flow tracing algorithm only needs to lock the geometric center coordinates of this block, reducing the originally massive computational load by three orders of magnitude, enabling the embedded processor to run in real time. Real-world test data shows that after this processing, the superpixel block identified as a leaf rapidly decays to a stationary state within 0.3 seconds after the airflow is cut off, exhibiting significant high-damping characteristics. In contrast, the superpixel block containing fruit maintains a pendulum-like oscillation with a period greater than 0.8 seconds for more than 1.5 seconds after the airflow stops. This quantified difference in dynamic data provides a highly confident basis for subsequent accurate removal of interference and identification of the fruit.

[0053] This invention utilizes inertial measurement unit (IMU) data for inverse kinematics calculation and electronic image stabilization, resolving the interference of unavoidable minor jitters on optical flow calculations caused by the robotic arm's hovering servo state. Without this step, the overall displacement of the background would be misinterpreted as low-frequency vibration, leading to tree trunks being mistaken for fruit. Simultaneously, superpixel segmentation technology is employed to cluster millions of pixels into hundreds of feature blocks for tracking. This preserves the object's edge structure information while reducing the computational load by several orders of magnitude, enabling complex time-frequency analysis algorithms to run in real-time on embedded edge computing devices.

[0054] Furthermore, the specific method for decoupling and classifying occluders based on time-frequency features to generate a semantic map is as follows: An amplitude threshold and an attenuation rate threshold are determined based on the environmental characteristics of the target orchard. The amplitude threshold ranges from 0.5 mm to 1.5 mm, and the attenuation rate threshold ranges from 0.3 to 0.5 mm. If the maximum amplitude of a superpixel block under airflow excitation is lower than the amplitude threshold, it is identified as a robust branch with high structural stiffness and marked as a rigid hard-constrained region. If the maximum amplitude of a superpixel block is higher than the amplitude threshold, and the damping attenuation rate after the airflow stops is lower than the attenuation rate threshold, it is identified as a fruit-bearing branch and / or fruit with high inertia and pendulum effect and marked as a vulnerable hard-constrained region. If the maximum amplitude of a superpixel block is higher than the amplitude threshold, and the damping attenuation rate after the airflow stops is higher than the attenuation rate threshold, it is identified as a leaf and marked as a soft-occlusion region.

[0055] The preset thresholds were determined based on extensive experimental data analysis of leaves, fruits, and branches in the target citrus orchard environment. Specifically, the displacement amplitude threshold was set to 1.0 mm (based on the average displacement of the geometric center of the superpixel block); the damping attenuation rate threshold was set to 0.4. Based on these thresholds, the classification logic is executed as follows:

[0056] First, robust branches possess extremely high structural rigidity, resulting in minimal displacement under the preset safe airflow excitation pressure. Therefore, the maximum amplitude is less than 1.0 mm, and regardless of frequency characteristics, it is directly classified as a rigid hard constraint. Second, for objects with amplitudes greater than 1.0 mm, damping characteristics are used for differentiation: fruits and fruit-bearing branches, due to their large mass and connection via flexible stalks, exhibit significant pendulum effects and high inertia retention, resulting in a long oscillation duration after the airflow stops. Therefore, the damping attenuation rate is less than 0.4, classifying them as vulnerable hard constraints. Finally, leaves and leaf clusters, due to their light mass and large wind-exposed area, experience significant air damping. Although a single small leaf typically exhibits high-frequency flutter (>5Hz), large leaves or leaf clusters may exhibit low-frequency oscillations similar to fruits (<2Hz). However, their common key physical characteristic is extremely rapid energy dissipation, resulting in rapid stillness after the airflow stops, thus exhibiting a damping attenuation rate greater than 0.4. Based on this high damping characteristic, regardless of the vibration frequency, they are classified as soft-blocked areas.

[0057] Specifically, after the pneumatic device sends a short pulse excitation lasting 0.5 seconds to the target area, the vision system's analysis of the time-series image reveals three typical dynamic behaviors:

[0058] First, for the lignified trunk with a diameter exceeding 20mm, the maximum displacement under airflow impact is only 0.6mm, far below the preset 1.0mm amplitude oscillation threshold. Based on the extremely low displacement response, the system directly determines it as an immovable and non-collision rigid constraint area. Second, for the fruit-bearing branches hidden about 15cm behind the leaves, due to the large weight of the single fruit (usually between 200g and 350g) and the flexible connection characteristics of the fruit stalk, the airflow excites a significant low-frequency pendulum-like motion, with a main oscillation frequency of only about 1.2Hz, but the maximum swing amplitude reaches 12mm (above the threshold). The key lies in its extremely strong inertial retention capability. Even after the airflow stops for 1.5 seconds, it maintains a visible periodic oscillation, with a calculated damping attenuation rate as low as 0.15 (below the threshold of 0.4). Based on this, the system identifies it as a vulnerable hard-constraint region and automatically generates an avoidance bounding box. Finally, in the field of view, not only were single young leaves exhibiting high-frequency flutter of 5Hz to 8Hz observed, but also some large leaf clusters formed by overlapping older leaves. The latter exhibited a large-amplitude oscillation at a low frequency of 1.8Hz (amplitude of about 8mm), which is easily confused with fruit. However, unlike fruit, due to the significant influence of air resistance, the leaf clusters rapidly dissipate their energy and return to rest within 0.4 seconds after the airflow stops. The calculated damping attenuation rates were 0.75 (single leaf) and 0.65 (leaf cluster), both significantly higher than the threshold of 0.4. Based on this, the system ignores the frequency difference and, relying solely on the inherent physical characteristic of high damping, uniformly and confidently marks the two types of high-amplitude objects as soft obstruction areas that allow the robotic arm to pass through, effectively avoiding accidental obstacle avoidance of large blade clusters.

[0059] The specific process of feature extraction and classification also includes three-dimensional scene flow analysis: combining the depth change rate of the depth vision sensor with the optical flow field of the RGB image, the motion vector of each superpixel region in three-dimensional space is calculated; based on the trajectory of the motion vector in the time dimension, a three-dimensional vibration probability ellipsoid model is constructed; the classification is assisted by analyzing the geometric morphological features of the vibration probability ellipsoid, where the flattened ellipsoid feature corresponds to the soft occlusion region constrained by the petiole, the spherical ellipsoid feature corresponds to the vulnerable hard constraint region of free suspension, and the linear ellipsoid feature corresponds to the rigid hard constraint region.

[0060] This invention upgrades the single "amplitude" feature to a "spatial morphology" feature by constructing a three-dimensional vibration ellipsoid, giving the classification algorithm stronger physical interpretability. This enables the system to accurately distinguish objects with similar motion patterns but completely different physical structures on a two-dimensional projection (such as distinguishing between a fruit swinging towards the front and a leaf swinging to the side) by utilizing differences in geometric morphology, significantly improving the recognition accuracy in complex occlusion environments.

[0061] This method uses damping decay rate as the core criterion for distinguishing between "lightweight leaves" and "heavyweight fruits." Regardless of the object's vibration speed, as long as its energy dissipates quickly (high damping), it indicates a lack of destructive inertial mass and can be defined as a soft obstruction. This improvement achieves complete coverage of the physical properties of obstructing objects, eliminates classification blind spots, ensures that the robot does not perform unnecessary obstacle avoidance actions on large leaves, and further improves harvesting efficiency.

[0062] Furthermore, the specific process of constructing a probabilistic grid map based on the semantic graph includes: constructing a 3D voxel grid map and initializing the prior probability log ratio of each grid; using the intrinsic parameter matrix of the depth vision sensor and the pose of the robotic arm's end effector, back-projecting the geometric center of each superpixel in the semantic graph, combined with its depth value, to the world coordinate system; using a ray casting model, marking the grid where the projection point is located as the occupied observation of the corresponding semantic category, and marking the grids on the path from the camera's optical center to the projection point as free space observations; using a Bayesian filtering algorithm, taking the current occupied observations and free space observations as input, recursively updating the posterior probability of each grid belonging to soft occlusion, vulnerable hard constraints, and rigid hard constraints. The algorithm employs a fully connected conditional random field (CRF) to globally optimize the 3D voxel map. By minimizing the energy function, which includes a univariate potential function based on posterior probability and a binary potential function based on color and spatial distance between voxels, spatial smoothing constraints are applied to the semantic labels of adjacent voxels, filtering out discrete noise points and filling semantic gaps caused by occlusion. When the posterior probability of a hard-constrained grid exceeds a safety threshold, the grid undergoes 3D morphological dilation during the generation of the virtual impedance map to construct a safety buffer. The free-space observation corresponds to a preset negative log-likelihood ratio in the Bayesian filtering update, and the log-likelihood ratio of the posterior probability of a grid belonging to any type of obstacle on the negative update path is used.

[0063] The observation model parameters in Bayesian filtering were obtained statistically from labeled datasets collected in a typical orchard environment. Specifically, the probability of correctly observing a true soft occlusion region as a soft occlusion was set to approximately 92%, the probability of correctly observing a true vulnerable hard constraint as a vulnerable hard constraint to approximately 88%, and the probability of correctly observing a true rigid hard constraint as a rigid hard constraint to approximately 95%. The average probability of inter-class misclassification was set to 1.5%. Based on these statistical probabilities, the log-likelihood ratio was calculated and used to recursively update the posterior probability of each grid cell. Meanwhile, in the fully connected conditional random field optimization, the weight coefficient of the univariate potential function was optimally set to 0.7 through cross-validation, and the weight ratio of spatial distance to color difference in the binary potential function was set to 1:1 to balance local observations and global consistency.

[0064] The Bayesian filtering algorithm described above updates the data using the log-likelihood ratio to improve computational efficiency and numerical stability. For each grid cell... It belongs to a certain semantic category posterior probability log ratio The update follows this rule: the log-probability ratio of the posterior probability at the current time step equals the log-probability ratio of the posterior probability at the previous time step plus the log-likelihood ratio of the observation model. Wherein, the observation model... In the actual state of the raster At that time, observations were obtained. The probability of misclassifying a soft occlusion as a hard constraint is determined by pre-calibrating the accuracy of the classifier. For example, the probability of correctly classifying a soft occlusion as a soft constraint is set to 0.9, corresponding to a positive log-likelihood ratio; the probability of misclassifying a soft occlusion as a hard constraint is set to 0.01, corresponding to a negative log-likelihood ratio. A fully connected conditional random field algorithm is introduced to globally optimize the 3D voxel map.

[0065] Specifically, the univariate potential function is linearly related to the logarithm of the Bayesian posterior probability of the current voxel, and its weighting coefficients... The value is set to 0.7. A binary potential function is used to constrain the semantic label consistency of adjacent voxels. A Gaussian kernel function is used to calculate the similarity between voxels, which is determined by the spatial distance between voxels in the world coordinate system and the L2 norm of their color differences. By minimizing the energy function, which includes both the univariate and binary potential functions, an iterative solution using a mean-field approximation algorithm is used to achieve spatial smoothness constraints on semantic labels.

[0066] Specifically, the working area, centered on the target fruit and spanning approximately 50 cm in depth, is first discretized into a 3D voxel mesh with a resolution of 5 mm × 5 mm × 5 mm. The prior probability logarithm ratio of all voxels is initialized to zero, representing maximum uncertainty. The sensor acquires data at a rate of 30 frames per second, and time-series accumulation is performed using Bayesian filtering. For example, if a voxel is continuously identified as a "rigid hard constraint" (tree branch), its occupancy probability will rapidly increase from 0.5 to over 0.95 within 10 frames. After the Bayesian update, the system initiates a Conditional Random Field (CRF) for global smoothing. CRF analyzes neighboring voxels. For example, if a voxel's Bayesian posterior probability indicates it is a "leaf" (soft occlusion), but all its neighbors are spatially continuous "rigid hard constraints," CRF's binary potential function applies a penalty, forcing the voxel's label to align with its neighbors' labels. This eliminates isolated "flying points" caused by depth jumps or rapid leaf flips (e.g., a voxel that originally belongs to a branch is momentarily misclassified as free space, but CRF corrects its probability through neighborhood constraints). Through CRF optimization, the final generated hard constraint contour becomes more continuous and smooth. Finally, once a confirmed hard constraint grid (probability > 0.98) is identified, the system performs a morphological dilation operation with a radius of 20 mm, expanding a 15 mm diameter twig into a 55 mm "no-go zone" on the virtual map. This provides the robotic arm with ±10 mm of positioning error redundancy, ensuring safe movement.

[0067] This invention addresses the noise and uncertainty issues inherent in instantaneous observation data by constructing a probabilistic grid map and incorporating a Bayesian filtering algorithm. In dynamic airflow environments, single observations may contain errors. Through recursive updates over time, the posterior probabilities of each grid cell belonging to different categories are continuously corrected, improving the confidence level of environmental modeling. Furthermore, three-dimensional morphological dilation of hard-constrained regions is applied to construct a safety buffer zone, effectively addressing sensor calibration errors and robotic arm control errors. This provides reliable spatial geometric constraints for subsequent obstacle avoidance movements, ensuring the robot's motion safety.

[0068] Furthermore, the specific method for constructing a low-impedance channel that allows passage through soft occlusion areas includes: extracting texture features from superpixel blocks identified as soft occlusion areas, calculating the main growth direction of the leaf texture using the image gradient direction histogram; constructing a three-dimensional impedance ellipsoid model, setting the major axis of the impedance ellipsoid to be parallel to the main growth direction of the leaf texture, and setting the minor axis of the impedance ellipsoid to be perpendicular to the main growth direction of the leaf texture; setting a first translational stiffness coefficient in the major axis direction to allow the robotic arm to generate compliant displacement along the leaf growth texture direction; and setting a second translational stiffness coefficient in the minor axis direction to limit the extrusion of the robotic arm perpendicular to the leaf texture direction.

[0069] The specific method for establishing a high-impedance repulsion field for grids identified as hard-constrained regions includes: constructing an artificial potential field repulsion model with the grid center identified as a vulnerable hard-constrained region or a rigid hard-constrained region as the potential field source point; for the rigid hard-constrained region, setting an isotropic first repulsion stiffness coefficient and a first repulsion radius to form a strong repulsion boundary; for the vulnerable hard-constrained region, setting an isotropic second repulsion stiffness coefficient and a second repulsion radius to form a protective repulsion field with a larger safety buffer; the first repulsion stiffness coefficient is greater than the second repulsion stiffness coefficient, and the second repulsion radius is greater than the first repulsion radius.

[0070] When constructing a low-impedance channel for soft-obstruction areas, the process also includes a robotic arm posture optimization step based on zero-space projection: utilizing the redundant degrees of freedom of the robotic arm, under the constraint of keeping the position and posture of the end effector unchanged, the joint angles of the robotic arm elbow and wrist are automatically adjusted in zero space; the axial direction of the robotic arm link is controlled to tend towards the main growth direction parallel to the blade texture, minimizing the equivalent windward cross-sectional area of ​​the robotic arm during the crossing process.

[0071] Attitude optimization fully utilizes the kinematic characteristics of redundant robotic arms, upgrading simple "force control" to "force-position composite optimization." By adjusting the attitude, the robotic arm cuts into the tree canopy with the smallest projected area, reducing the probability of physical contact with surrounding branches and leaves. This not only further reduces crossing resistance in conjunction with anisotropic impedance strategies, but also effectively prevents the robotic arm elbow from accidentally snagging on surrounding hard branches during movement, improving the flexibility and safety of operations in confined spaces.

[0072] Specifically, for example, when considering a large citrus leaf obscuring the target fruit, the image processing module calculates that the main growth direction of its veins is drooping downwards at a 45-degree angle to the horizontal plane. Based on this, the controller constructs an anisotropic impedance model in Cartesian space: along the vein growth direction (i.e., the major axis of the impedance ellipsoid), the translational stiffness coefficient is set to 30 N / m, which means that the robotic arm only needs a very small external force (about 0.5 Newtons) to produce compliant sliding in this direction; however, in the direction perpendicular to the leaf surface (minor axis), the stiffness is maintained at 500 N / m to prevent the robotic arm from excessively squeezing the leaf, causing uncontrollable deformation of the hidden branches behind it.

[0073] Meanwhile, for the identified hard, dead branch with a diameter of 2 cm, a rigid repulsion field stiffness of 3000 N / m and a dense repulsion radius of 2.5 cm were set, which is equivalent to building an "invisible steel wall" to ensure that the end effector of the robotic arm can never intrude into its physical boundary. For a nearby tender green fruit, although it is also a hard constraint, in order to prevent scratching the fruit peel, its repulsion radius is expanded to 6 cm, forming a "flexible airbag" with a wider range but moderate stiffness (1500 N / m). This forces the robotic arm to begin to smoothly change its trajectory at a greater distance from the green fruit. This finely layered potential field design realizes the intelligent strategy conversion from "absolute collision avoidance" to "flexible avoidance".

[0074] This invention identifies the texture growth direction in soft occlusion areas and establishes a low-impedance channel along the texture direction, allowing the robotic arm to compliantly traverse like "gliding" over a leaf, while maintaining high impedance in the direction perpendicular to the texture to prevent excessive compression. This anisotropic modeling strategy avoids a one-size-fits-all "detour" approach to all visual obstructions, significantly shortening the robotic arm's movement path and reducing redundant obstacle avoidance actions, thereby significantly improving the efficiency of harvesting operations in complex, unstructured citrus environments.

[0075] Furthermore, adjusting the Cartesian stiffness and damping parameters of the robotic arm specifically includes: when the robotic arm is in a soft-obstruction area, the translational stiffness coefficient in the Cartesian stiffness matrix is ​​set to a first preset stiffness value. When the robotic arm is subjected to the contact force of the blade, the actual position relative to the desired position generates a compliant displacement, which absorbs the collision energy and slides along the blade surface. When the robotic arm approaches a hard-constrained area, the translational stiffness coefficient in the Cartesian stiffness matrix is ​​set to a second preset stiffness value, and a virtual repulsive force based on the gradient of the artificial potential field is superimposed. The position deviation pointing towards the hard-constrained area generates a reverse correction force in the controller to overcome the inertia of the robotic arm and external interference.

[0076] The principle for selecting the first preset stiffness value is that it should be set within a low range to ensure that the contact force generated when the robotic arm makes physical contact with the leaf is less than the damage threshold of the leaf tissue or the connection point of the fruit stalk, while allowing the robotic arm to generate sufficient compliant sliding displacement on the leaf surface. The principle for selecting the second preset stiffness value is that it should be set within a high range to generate a sufficiently large counter-correction force to overcome the inertial momentum of the robotic arm at its maximum operating speed, ensuring that the robotic arm can effectively brake and change its trajectory before reaching the safety buffer zone boundary of the hard constraint area.

[0077] In the virtual impedance map, a stiffness transition zone with a preset thickness of 15mm to 25mm is defined at the interface between the soft obstruction region and free space. Within the stiffness transition zone, a stiffness change curve is constructed using an S-curve function with the distance from the end of the robotic arm to the boundary of the soft obstruction region as the independent variable. As the end of the robotic arm moves from free space into the soft obstruction region, the Cartesian space stiffness parameter is controlled to continuously and monotonically decrease along the S-curve function to a preset crossing stiffness value.

[0078] Specifically, as the robotic arm's end effector moves from free space to the soft-blocked region, the system first defines a stiffness transition band on the virtual impedance map, for example, with a width of 20 mm. This transition band is crucial for connecting the high-stiffness free space and the low-stiffness crossing region. The controller uses an S-curve function to construct the stiffness change curve to ensure that the stiffness parameter can continuously and smoothly decrease from the high stiffness value in free space (e.g., 2000 N / m) to the low-stiffness crossing value in the soft-blocked region (e.g., 60 N / m).

[0079] The construction of a sigmoid function satisfies the following two key boundary conditions and transition properties:

[0080] Smooth connection at free-space boundary: When the end effector of the robotic arm is located at the outer boundary of the transition zone (20 mm from the soft shielding area), the value of the stiffness curve is exactly equal to the high stiffness value in free space. At this boundary point, the rate of change of stiffness with position is set to zero. This ensures that when the robotic arm enters the transition zone from the high stiffness mode, the stiffness change starts from a smooth value and a zero rate of change, avoiding sudden "jumps" in stiffness parameters.

[0081] Smooth transition across the crossing zone: When the robotic arm's end effector moves to the inner boundary of the transition zone (i.e., upon contact with the soft barrier area), the stiffness curve value is precisely equal to the preset low-stiffness crossing value. Simultaneously, at this inner boundary point, the rate of change of stiffness with position is again set to zero. This ensures that when the robotic arm truly enters compliant crossing mode, the stiffness change has completely subsided, allowing it to begin performing the low-stiffness compliant crossing operation in the smoothest possible state.

[0082] The S-curve constructed in this way allows the stiffness of the robotic arm to exhibit a smooth, monotonous decreasing trend of "slow-fast-slow" within a distance of 20 millimeters, completely eliminating the impact and oscillation that sudden changes in stiffness may cause to the control system, and ensuring the anthropomorphism and high stability of the entire variable stiffness control process.

[0083] The nominal stiffness of the free space is set to 2000 N / m to ensure trajectory tracking accuracy during high-speed approach. When the robotic arm's end effector enters the stiffness transition zone 20 mm from the soft obstruction area boundary, the controller does not abruptly change the parameters. Instead, it continuously and smoothly decays the Cartesian translation stiffness to the first preset stiffness value, 60 N / m, within a 50-m control cycle according to an S-shaped function curve. In this low-stiffness mode, when the robotic arm makes physical contact with an old leaf with a thickness of 0.3 mm, only a weak reaction force of 1.5 N is needed to force the end effector to deviate from the preset trajectory by about 10 mm. This proactive "weakening" characteristic allows the robotic arm to slide along the geometric surface of the leaf, avoiding leaf tearing or robotic arm overload alarms caused by rigid confrontation.

[0084] Conversely, when the end effector moves into a dangerous neighborhood of 15mm from the rigid tree branch, the controller instantly increases the stiffness parameter to the second preset stiffness value, 4000N / m, while simultaneously applying a virtual repulsive force generated by an artificial potential field. Assuming the robotic arm continues to approach the branch due to inertia, it will generate a reverse corrective force of up to 45 Newtons. This force is much greater than the robotic arm's inertial impulse at this time, thus forcing the robotic arm to decelerate and change its motion vector within 5 milliseconds. This ensures that the minimum distance between the robotic arm and the rigid obstacle is always maintained above the 5mm safety limit, achieving a millisecond-level modal switch from "flexible crossing" to "rigid obstacle avoidance".

[0085] This invention achieves dynamic adaptive adjustment of stiffness in Cartesian space, resolving the contradiction between "flexibility" and "rigidity" when a robot interacts with its environment. When traversing soft obstructions, stiffness is smoothly reduced via an S-curve, utilizing the robot arm's compliance to absorb collision energy, simulating the "push-aside" action of a human picking fruit. Conversely, when approaching hard constraint areas, stiffness is increased and a virtual repulsive force is superimposed, using the controller's reverse correction force to overcome inertia and enforce obstacle avoidance. This variable stiffness control strategy ensures both compliance when traversing blades and rapid response during obstacle avoidance, achieving the optimal effect of force-position hybrid control.

[0086] Furthermore, achieving compliant passage through soft obstruction areas also includes setting a joint torque saturation threshold. The specific logic is as follows: during the process of the robotic arm passing through the soft obstruction area, the current value of each joint motor is monitored in real time, and the external contact torque is estimated; a floating threshold is set that dynamically changes with the theoretical contact torque caused by passing through the blade, the floating threshold being higher than the blade contact torque but lower than the branch destruction torque; when the monitored rate of change of external contact torque exceeds the floating threshold, it is determined that an invisible hard obstacle has been touched, the controller immediately switches the robotic arm to gravity compensation mode, and marks the obstacle position as a new hard constraint point to update the semantic graph.

[0087] Specifically, a high-frequency current loop monitoring mechanism is employed, using an EtherCAT bus to read the current feedback data of each joint of the robotic arm in real time at a frequency of 2000Hz. When the end effector of the robotic arm traverses the dense blade layer at a speed of 0.1 m / s, due to the good flexibility of the blades, the resulting contact resistance typically exhibits a slow and low-amplitude fluctuation. The measured external contact torque remains between 0.5 N·m and 1.5 N·m, with a torque change rate of less than 5 N·m / s. Based on this benchmark data, the controller dynamically sets the floating threshold for safety monitoring to three times the current contact torque benchmark value, with the change rate threshold limited to 20 N·m / s.

[0088] Suppose there is an invisible dead branch with a diameter of 8mm that is not visually detected in the path. When the robotic arm's forearm makes a rigid collision with it, the joint motor current will surge in a pulse. Within two control cycles (i.e., 1 millisecond), the algorithm detects that the estimated rate of change of external contact torque instantly spikes to 60 N·m / s, far exceeding the preset safety threshold of 20 N·m / s. Although the absolute torque value has not yet reached the limit load for branch breakage (about 15 N·m), the controller determines it as a rigid collision based on the extremely high rate of change and immediately triggers the "soft emergency stop" logic in the next control cycle, switching the servo control mode from position control to gravity compensation mode. The robotic arm instantly loses rigidity and droops naturally under the action of gravity or moves backward with the reaction force. The encoder position information at the moment of collision is read, and a hard constraint obstacle point with a radius of 15mm is added at the corresponding coordinates in the semantic map. In subsequent planning, a detour trajectory is automatically generated, thereby realizing tactile active safety defense in the visual blind spot.

[0089] This invention constructs a final active safety defense against invisible obstacles. When the visual system is completely obscured by leaves and cannot detect hidden branches, relying on a single visual approach may lead to serious collisions. This invention, by monitoring the joint torque change rate in real time and setting a floating threshold that dynamically changes with the theoretical contact torque, can detect abnormal changes in milliseconds during accidental rigid contact. By immediately switching to gravity compensation mode, the robotic arm instantly unloads the force, minimizing mechanical damage to the precision mechanical structure and fruit tree branches, and greatly improving its self-protection capability under extreme working conditions.

[0090] This invention provides a non-contact active sensing and control architecture, solving the problem of low efficiency and vulnerability caused by the "blind exploration" of traditional harvesting robots in complex, obstructed environments. By combining pneumatic pulse excitation with time-frequency feature analysis, this method can accurately decouple the environment into soft obstructions (leaves), vulnerable hard constraints (fruits), and rigid hard constraints (branches) before physical contact occurs. This prior physical property perception capability enables the robot to intelligently decide whether to "cross" or "detour," thereby significantly improving operational efficiency and path planning success rate in unstructured environments while ensuring the safety of fruits and equipment.

[0091] Example 2

[0092] This embodiment focuses on demonstrating the specific application of the present invention in dealing with natural environmental disturbances in open-air orchards and in scenarios with deep and complex shading. In such scenarios, the target citrus fruits typically grow in the lower part of the tree canopy, heavily enveloped by outer old leaves, middle new shoots, and adjacent immature fruits. Furthermore, natural winds cause irregular low-frequency swaying of the branches and leaves, placing higher demands on the robustness of the "non-contact" active sensing and control strategy. This embodiment achieves precise operation under dynamic disturbances through the organic combination of electronic image stabilization, adaptive aerodynamic excitation, Bayesian probability updating, and anisotropic impedance control.

[0093] As one embodiment of the present invention, refer to Figure 1 A flowchart of a motion control method for a citrus harvesting robot based on occlusion estimation, referring to... Figure 2 A schematic diagram of occlusion classification decision based on time-frequency characteristics and vibration ellipsoid, referring to... Figure 3 A schematic diagram of the robot's Cartesian space stiffness mode switching state.

[0094] First, the mobile robot chassis navigates to the front of the target fruit tree, and the robotic arm, carrying the end effector, moves to a position approximately 40 centimeters in front of the region of interest (ROI) to be identified and hovers there. Considering the natural wind interference in the open-air environment and the varying depths at which the target fruit is hidden, a simple constant air pressure is insufficient for detection. The controller first activates the depth vision sensor to acquire a depth distribution cloud map of the surface of the obstructions within the current field of view.

[0095] The controller reads the real-time depth distance of the center point of the obstruction and calculates the target output power of the aerodynamic excitation device using a preset nonlinear mapping relationship. For outer blades that are close to the lens (e.g., 20 cm away), the controller outputs a low primary pressure, which is sufficient to overcome the static inertia of the blades and induce high-frequency flutter, but is strictly controlled within the bending strength range of the petiole to avoid strong airflow damaging tender branches and leaves or blowing away nearby shallow fruits. At the same time, for deep obstructions that are far from the lens (e.g., 50 to 60 cm away), the controller significantly increases the outlet pressure of the aerodynamic device according to a quadratic trend increasing logic. Through this pressure equalization control across the entire depth range, obstructions at all depths within the field of view can be activated non-contactly and uniformly, causing them to exhibit observable physical vibration characteristics, thus avoiding missed detections due to insufficient deep excitation or accidental damage due to excessive shallow excitation.

[0096] High-speed image acquisition is initiated, and an electronic image stabilization strategy is implemented to eliminate servo jitter during robotic arm hovering and background shaking caused by natural wind. Simultaneously, the high-frequency acceleration and angular velocity data of the end effector's six degrees of freedom are read from the inertial measurement unit. The tiny displacement vector of the camera's optical center at each exposure moment is calculated in real time through inverse kinematics calculation. By using this vector to perform pixel-level inverse translation and rotation compensation on the original image stream, the background is frozen, eliminating the interference of the robotic arm's own tremor on the optical flow calculation.

[0097] Subsequently, superpixel segmentation was performed on the stabilized image, and time-frequency analysis of the displacement signals of each superpixel block before, during and after airflow excitation was performed throughout the entire cycle. The low-frequency large-amplitude swaying of leaves caused by natural wind (usually <1Hz) was baseline removed by using a spectrum filter, and the feature response caused by aerodynamic pulses was extracted. Based on the decoupling classification logic, the core features were extracted: main oscillation frequency, amplitude and damping attenuation rate.

[0098] For soft obstructions, a high dominant frequency, such as greater than 5Hz, was detected under airflow impact. At the instant the airflow was cut off, the vibration amplitude showed an extremely high damping attenuation rate. For vulnerable hard constraints, although the vibration frequency was low, an extremely low damping attenuation rate was detected after the airflow stopped. This was due to the inertial effect caused by the large mass of the fruit, which resulted in a long-term periodic swaying similar to a simple pendulum. For rigid hard constraints, the structure exhibited extremely high stiffness regardless of the air pressure adjustment, and the displacement amplitude was always lower than the vibration threshold.

[0099] Considering that a single observation may be affected by sudden changes in illumination or leaf flipping, this embodiment uses a Bayesian filtering algorithm to construct a three-dimensional probabilistic grid map; the field of view is discretized into a voxel grid, and the semantic classification result of each observation is back-projected into the grid using a ray casting model; for grids that are judged as rigid hard constraints, their posterior probability log ratio is recursively updated based on the observation results of multiple consecutive frames; as the observation time accumulates, the probability of actual obstacle occupancy quickly converges to the high confidence interval, while instantaneous visual noise is filtered out with low probability.

[0100] Once a grid is identified as a hard constraint region, it undergoes 3D morphological dilation during navigation map generation. A virtual safety buffer layer is constructed by extending outward from the branch skeleton by a preset safety radius (e.g., 2 cm). This buffer layer is considered an insurmountable no-go zone in subsequent potential field modeling, aiming to compensate for the positioning error of the robotic arm and the calibration residual of the vision system, ensuring that a physical safety gap is always maintained between the physical robotic arm and the hard tree trunk.

[0101] After completing the environmental attribute perception, the path planning stage begins. In this embodiment, the identified dense leaf layer is no longer regarded as an obstacle, but a low-impedance crossing channel is constructed. Texture analysis is performed on the superpixel blocks of the leaf area, and the main growth direction of the leaf veins is accurately extracted by calculating the image gradient direction histogram.

[0102] Based on this direction, an anisotropic impedance ellipsoid model is constructed in Cartesian space. In the direction of the major axis parallel to the leaf vein texture, the controller sets an extremely low translational stiffness coefficient, making the robotic arm exhibit "soft" characteristics in this direction, allowing it to slide compliantly along the texture under minimal external force. In the direction of the minor axis perpendicular to the leaf surface, a higher stiffness coefficient is set to limit the robotic arm from laterally squeezing the leaf. This strategy simulates the action of a human hand inserting into the gaps in the leaves when picking, greatly reducing the resistance to crossing and avoiding motion jamming caused by leaf stacking.

[0103] For both fragile and rigid hard constraints, a high-impedance artificial potential field repulsion model is established with the grid center as the source point. Among them, the rigid branches are set as strong repulsion nuclei with extremely high stiffness coefficient and small radius of action. The fragile fruit is set as a protective repulsion field with a large radius of action but slightly lower stiffness, forming a flexible "protective airbag" that forces the robotic arm to smoothly detour in advance to prevent scratching the fruit surface.

[0104] Finally, during motion control, the controller adjusts the dynamic parameters of the robotic arm in real time. When the end of the robotic arm cuts into the soft-blocked area from free space, the Cartesian space stiffness parameter smoothly decreases along the S-shaped function curve, achieving a seamless switch from high-stiffness positioning to low-stiffness traversal. During the traversal, the robotic arm uses its low-stiffness characteristics to absorb the reaction force of the leaves and smoothly slides into the depths of the tree canopy.

[0105] To address potential hidden hard obstacles within blind spots, such as dead branches completely obscured by large leaves, a torque saturation defense mechanism is activated throughout the entire crossing process. The controller monitors the rate of change of current in each joint motor in real time and estimates the external contact torque using a dynamic model. Once the rate of change of contact torque is detected to momentarily exceed a floating threshold dynamically set according to the theoretical blade resistance, an unexpected rigid collision is immediately identified. At this point, the controller forcibly interrupts the current trajectory interpolation within milliseconds, switches the control mode to gravity compensation mode, and causes the robotic arm to instantly unload the force and retreat in the direction of the collision. Subsequently, the coordinates of the collision point are marked as a new hard constraint obstacle, updated in the probabilistic grid map, and a local replanning is triggered. This constitutes a dual safety closed loop of visual feedforward perception and tactile feedback protection, ensuring operational safety and robustness in highly complex unstructured environments.

[0106] This embodiment significantly improves the robot's robustness in outdoor natural interference environments. Through depth-adaptive aerodynamic excitation and electronic image stabilization technology, it effectively counteracts the negative impact of natural wind interference and robotic arm vibration on perception, achieving non-destructive and accurate detection within the full depth range. By constructing a probabilistic map using Bayesian filtering and filtering out environmental noise through temporal accumulation, it improves the confidence level of deep target recognition. In particular, the combination of anisotropic traversal strategy based on texture direction and torque saturation defense mechanism not only enables efficient "slide-in" picking in accordance with leaf vein direction, greatly shortening the operation path, but also constructs a millisecond-level tactile safety defense line in the visual blind spot, completely solving the collision risk caused by invisible obstacles, and significantly improving the picking efficiency and system safety in complex unstructured scenarios.

[0107] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A citrus picking robot motion control method based on occlusion estimation, characterized by, The method comprises the following steps: The controller controls the mechanical arm to hover in front of the shielding area, reads the real-time depth distance of the center point of the shielding object, calculates the target output power of the pneumatic excitation device by using a preset nonlinear mapping relationship, and applies a pneumatic pulse; Collecting a time sequence image stream of the shielding area; Segmenting the image into superpixel regions, performing time-frequency analysis on the displacement signals of each superpixel region, extracting the main vibration amplitude and damping attenuation rate, decoupling and classifying the soft shielding area, the easily-damaged hard constraint area and the rigid hard constraint area according to the time-frequency characteristics, and generating a semantic atlas; Based on the semantic atlas, a probability grid map is constructed, the region attribute probability of each grid is updated by using a Bayesian filter, a low-impedance channel allowing to pass through is constructed according to the texture direction for the soft shielding area, and anisotropic expected impedance parameters are set; a high-impedance virtual repulsion field is constructed for the easily-damaged hard constraint area and the rigid hard constraint area, and high-impedance repulsion field parameters are set; and a virtual impedance map is generated based on the low-impedance channel and the high-impedance repulsion field; According to the real-time position of the end of the mechanical arm in the virtual impedance map, the controller adjusts the Cartesian space stiffness and damping parameters of the mechanical arm by using the expected impedance parameters and the high-impedance repulsion field parameters.

2. The motion control method of a citrus picking robot based on occlusion estimation according to claim 1, wherein, The specific mode of the controller for adaptively adjusting the output power of the pneumatic excitation device is as follows: an expected impact pressure threshold of the target shielding object surface is set; the controller reads the depth distance of the center point of the shielding object fed back by the depth vision sensor in real time; a nonlinear mapping relationship between the output power and the depth distance is constructed, specifically, the corresponding relationship between the outlet pressure of the pneumatic excitation device and the depth distance is determined through pre-experiment calibration, so that the airflow impact force reaching the shielding object surface at different depths remains within the preset effective excitation range after the airflow is diffused in the air and the kinetic energy is lost.

3. The motion control method of a citrus picking robot based on occlusion estimation according to claim 1, wherein, The collection of the time sequence image stream of the shielding area further comprises electronic image stabilization processing of the image stream: six-degree-of-freedom jitter data of the end of the mechanical arm collected by an inertial measurement unit is obtained, the instantaneous displacement vector of the camera optical center is calculated through inverse kinematics calculation, and the time sequence image frames collected are pixel-level compensated in reverse to eliminate the background global displacement caused by the servo jitter of the mechanical arm hovering; The specific steps of superpixel segmentation comprise the following steps: a first frame of image after image stabilization is processed by using a simple linear iterative clustering algorithm, pixels with similar color and spatial distance are aggregated into superpixel blocks, and the geometric center of the superpixel block is taken as a tracking feature point for optical flow tracking of subsequent frames; the Hilbert-Huang transform algorithm is used to analyze the non-stationary signal of the displacement time sequence signal obtained through tracking, and the instantaneous frequency and instantaneous energy distribution are extracted; the maximum amplitude is extracted based on the maximum value of the instantaneous energy distribution; the starting vibration response at the moment of airflow contact is identified according to the jumping characteristics of the instantaneous frequency, and the damping attenuation rate is calculated according to the energy attenuation characteristics of the Hilbert spectrum.

4. The motion control method of a citrus picking robot based on occlusion estimation according to claim 1, wherein, The specific method for decoupling and classifying the occlusion according to the time-frequency characteristics and generating a semantic atlas comprises the following steps: presetting an amplitude threshold and a damping rate threshold; if the maximum amplitude of a superpixel block under air flow excitation is lower than the amplitude threshold, the superpixel block is determined to be a thick branch with high structural stiffness, and is marked as a rigid hard constraint area; if the maximum amplitude of the superpixel block is higher than the amplitude threshold, and the damping attenuation rate after air flow stops is lower than the damping rate threshold, the superpixel block is determined to be a fruit-bearing branch and / or fruit with a pendulum effect, and is marked as a vulnerable hard constraint area; if the maximum amplitude of the superpixel block is higher than the amplitude threshold, and the damping attenuation rate after air flow stops is higher than the damping rate threshold, the superpixel block is determined to be a leaf, and is marked as a soft occlusion area.

5. The motion control method of a citrus picking robot based on occlusion estimation according to claim 1, wherein, The specific process of constructing a probabilistic grid map based on the semantic atlas comprises the following steps: constructing a three-dimensional voxel grid map and initializing the prior probability logarithmic ratio of each grid; using the intrinsic matrix of the depth vision sensor and the end pose of the manipulator, the geometric center of each superpixel in the semantic atlas is combined with the depth value to be back projected to the world coordinate system; using a ray casting model, the grid where the projection point is located is marked as an occupancy observation of the corresponding semantic category, and the grid on the path from the camera optical center to the projection point is marked as a free space observation; using a Bayesian filtering algorithm, the current occupancy observation and the free space observation are taken as inputs to recursively update the posterior probability logarithmic ratio of each grid belonging to a soft occlusion, a vulnerable hard constraint and a rigid hard constraint; when the posterior probability of a hard constraint grid exceeds a safety threshold, the grid is subjected to three-dimensional morphological dilation processing when a virtual impedance map is generated, and a safety buffer is constructed; the free space observation corresponds to a preset negative log-likelihood ratio in the update of the Bayesian filtering, and the posterior probability logarithmic ratio of the grid on the path belonging to any type of obstacle is updated negatively.

6. The motion control method of a citrus picking robot based on occlusion estimation according to claim 1, wherein, The specific method for constructing a low-impedance channel allowing traversal for the soft occlusion area comprises the following steps: performing texture feature extraction on the superpixel block of the soft occlusion area, and calculating the main growth direction of the leaf texture using an image gradient direction histogram; constructing a three-dimensional impedance ellipsoid model, and setting the long axis direction of the impedance ellipsoid to be parallel to the main growth direction of the leaf texture, and setting the short axis direction of the impedance ellipsoid to be perpendicular to the main growth direction of the leaf texture; setting a first translation stiffness coefficient in the long axis direction to allow the manipulator to produce a compliant displacement along the leaf growth texture direction; setting a second translation stiffness coefficient in the short axis direction to limit the extrusion of the manipulator perpendicular to the leaf texture direction; the first translation stiffness coefficient is lower than a first preset stiffness value, and the second translation stiffness coefficient is higher than a second preset stiffness value; The specific method for establishing a high-impedance repulsion field for the grid determined to be a hard constraint area comprises the following steps: taking the grid centers of the vulnerable hard constraint area and the rigid hard constraint area as potential field source points to construct an artificial potential field repulsion model; for the rigid hard constraint area, setting an isotropic first repulsion stiffness coefficient and a first repulsion action radius to form a strong repulsion boundary; for the vulnerable hard constraint area, setting an isotropic second repulsion stiffness coefficient and a second repulsion action radius; the first repulsion stiffness coefficient is greater than the second repulsion stiffness coefficient, and the second repulsion action radius is greater than the first repulsion action radius.

7. The method of claim 1, wherein, The adjusting the Cartesian space stiffness and damping parameters of the mechanical arm specifically includes: when the mechanical arm is in the soft occlusion area, setting the translational stiffness coefficient in the Cartesian space stiffness matrix as a first preset stiffness value, when the mechanical arm is subjected to the blade contact force, the actual position produces a compliance displacement relative to the expected position, the compliance displacement is used to absorb the collision energy and slide along the blade surface; when the mechanical arm approaches the hard constraint area, the translational stiffness coefficient in the Cartesian space stiffness matrix is set as a second preset stiffness value, and a virtual repulsive force based on the artificial potential field gradient is superimposed, the position deviation of the hard constraint area generates a reverse correction force in the controller, which overcomes the inertia of the mechanical arm and external disturbance; In the virtual impedance map, a stiffness transition zone with a preset thickness is defined at the interface of the soft occlusion area and the free space; in the stiffness transition zone, the distance from the end of the mechanical arm to the boundary of the soft occlusion area is used as the independent variable, and the S-shaped function is used to construct the stiffness change curve; in the process of the end of the mechanical arm entering the soft occlusion area from the free space, the Cartesian space stiffness parameters are continuously and monotonously reduced to a preset penetration stiffness value along the S-shaped function, ensuring that the mechanical arm enters the compliant penetration mode.

8. The motion control method of a citrus picking robot based on occlusion estimation according to claim 7, characterized in that, The compliant penetration in the soft occlusion area also includes setting a joint torque saturation threshold, specifically: in the process of the mechanical arm penetrating the soft occlusion area, the current values of each joint motor are monitored in real time, and the external contact torque is estimated, including the blade contact torque and the branch damage torque; A floating threshold is set, which is higher than the blade contact torque but lower than the branch damage torque; when the monitored external contact torque change rate exceeds the floating threshold, it is determined that the invisible hard obstacle is touched, the controller immediately switches the mechanical arm to the gravity compensation mode, and the obstacle position is marked as a new hard constraint point updated to the semantic map.

Images