Control method of citrus picking robot based on dual-view cooperation and visual servoing

Abstract

The application discloses a kind of based on the control method of orange picking robot of double vision coordination and visual servoing, include: starting global depth camera acquisition fruit tree point cloud data;Drive mechanical arm to end delivery preparatory work point;Based on local micro-lens camera extraction fruit stalk feature vector, establish characteristic state smooth updating equation, construct visual depth scale coupling adaptive gain and smooth dynamic damping, combined with feedforward prediction in singular value level expansion solution generation camera space speed instruction vector;Real-time calculation space alignment degree, attitude matching degree and depth approximation degree confidence sub-score, calculate final shear confidence and execute shear.The control method of orange picking robot based on double vision coordination and visual servoing of the present application constructs the double-layer control framework of global coarse positioning plus local fine servoing, effectively solves the problems of positioning error accumulation in existing open-loop control mode, unable to cope with dynamic environment, easy to damage fruit and forcibly withdraw arm to break off branch.

Description

Technical Field

[0001] This invention belongs to the field of agricultural robot technology, specifically relating to a control method for a citrus harvesting robot based on dual-view collaboration and visual servoing. Background Technology

[0002] With the development of agricultural automation technology, automated citrus harvesting robots have become an important technical means to improve orchard operation efficiency and reduce labor costs. The current control system of automated citrus harvesting robots mainly adopts the "monocular / binocular global vision + open-loop control" mode: the robot only relies on the global camera mounted on the chassis or gimbal to take pictures of the orchard. After the vision system calculates the three-dimensional coordinates of the fruit, it performs inverse kinematics calculation, plans a trajectory, and drives the robotic arm to move directly to the coordinate point to perform grasping or cutting. During the movement of the robotic arm, the system no longer corrects the target position.

[0003] However, the above-mentioned open-loop control method has serious drawbacks in actual unstructured orchard environments:

[0004] Accumulated positioning errors lead to harvesting failures. Because the global camera is far from the fruit (usually >0.5 meters), the depth measurement is noisy, and the robotic arm experiences accumulated joint errors and chassis vibrations during long-distance movement. As a result, the end point often has a centimeter-level deviation when it reaches the target point, making it impossible to accurately align with the fruit stem, thus causing harvesting failures.

[0005] Unable to cope with dynamic environments. In outdoor environments, natural wind blowing branches or robotic arms touching leaves can cause real-time shifts in the position of fruit. Open-loop control systems cannot detect these dynamic changes and continue to execute according to the original coordinates, which can easily lead to "missed grab" or "off-center shearing".

[0006] It can easily damage the fruit. Due to the lack of close-range fine-tuning, the end effector often cannot guarantee that it will intervene at the optimal angle (such as perpendicular to the fruit stem). If the position is slightly off, the blade can easily cut the flesh or pinch the peel.

[0007] Therefore, there is an urgent need for a control method for citrus harvesting robots that can overcome insufficient global positioning accuracy, have real-time correction capabilities, and achieve non-destructive harvesting. Summary of the Invention

[0008] This invention provides a control method for a citrus harvesting robot based on dual-view collaboration and visual servoing to solve the aforementioned technical problems. Specifically, the technical solution is as follows:

[0009] A control method for a citrus harvesting robot based on dual-view collaboration and visual servoing includes the following steps:

[0010] The global depth camera is activated to collect point cloud data of fruit trees. The bounding box of mature citrus fruits is identified based on the target detection model. The rough three-dimensional coordinates of the fruit center in the base coordinate system are calculated by combining the depth information.

[0011] Set a pre-operation point, perform inverse kinematics calculation to plan a collision-free trajectory, drive the robotic arm to move quickly to deliver the end effector to the pre-operation point, and transfer system control to the local macro camera.

[0012] Based on the extraction of fruit stem feature vectors by local macro camera, the occlusion confidence is introduced to establish a smooth update equation for feature state, an adaptive gain coupled with visual depth scale is constructed, and smooth dynamic damping is constructed based on the singular value decomposition of the image Jacobian matrix. The solution is carried out at the singular value level to generate camera spatial velocity command vector, which drives the differential motion of the robotic arm to correct the end posture in real time.

[0013] The confidence sub-scores of spatial alignment, pose matching and depth approximation are calculated in real time. The final shear confidence is calculated using a multiplicative fusion mechanism. When the shear confidence reaches the threshold and the duration exceeds the stable time window, the shear enable signal is output.

[0014] After the shearing is completed, the verification frame image is acquired after a delay, the feature residual rate is calculated, and the state transition is performed according to the residual rate interval to complete the closed-loop verification and anomaly scheduling.

[0015] Furthermore, the step of activating the global depth camera to collect fruit tree point cloud data includes: activating the global RGB-D camera in a stationary state, loading a target detection model trained on a real orchard multidimensional dataset, wherein the multidimensional dataset contains positive and negative samples of images of the fruit taken by the camera under different lighting conditions, at different distances between the camera and the fruit, and from the perspectives of eye level, top view, and bottom view, identifying the bounding box of the mature citrus and calculating the rough three-dimensional coordinates of the fruit center in the base coordinate system.

[0016] Furthermore, the setting of the preparatory work point includes: the preparatory work point is located at a specific distance from the fruit surface along the line connecting the centers of the fruit. This point serves as a switching node between global coarse positioning and local fine servoing, realizing the handover of control in the dual-view collaborative architecture and switching the system from open-loop control to closed-loop visual servo control.

[0017] Furthermore, the extraction of fruit stalk feature vector based on local macro camera includes: extracting fruit stalk feature vector through semantic segmentation, wherein the feature vector includes pixel coordinates, logarithm of cross-sectional area, and principal axis tilt angle; and introducing occlusion confidence based on image mask integrity, wherein the occlusion confidence value ranges from 0 to 1.

[0018] Furthermore, the establishment of the feature state smooth update equation includes: introducing occlusion confidence to establish a first-order Markov smooth update mechanism, and establishing the feature state smooth update equation:

[0019] ;

[0020] Where ρ is the occlusion confidence score calculated from the pixel integrity of the local visual mask, and its value range is [0,1]; s visual The observed feature vector extracted by the camera in the current frame; s t-1 and These represent the characteristic state and characteristic rate of change of the previous moment, respectively; Δt is the sampling period of the control system.

[0021] When the fruit is momentarily obscured by leaves, ρ approaches 0, and the system relies entirely on historical states and rates of change for inertial extrapolation; when the field of vision is clear, ρ approaches 1, and the system is dominated by real-time observation, cutting off the robotic arm position step commands caused by the momentary loss of visual features.

[0022] Furthermore, the adaptive gain for constructing visual depth scale coupling includes:

[0023] By implicitly representing the real physical distance using the cross-sectional area of ​​the fruit stalk, a dynamic gain coefficient is constructed:

[0024] ;

[0025] Where, λ min With λ max Here, represents the minimum and maximum servo gain allowed by the system, respectively; α is the attenuation slope coefficient; e is the deviation between the current image feature deviation and the desired feature, and ||e|| is the Euclidean norm of the deviation between the current image feature deviation and the desired feature; A img Let A be the current characteristic cross-sectional area of ​​the fruit stalk. ref The target reference cross-sectional area;

[0026] When the camera is far from the target, A img The gain is extremely small, and the system forcibly reduces the overall gain. Even if the target experiences a large pixel deviation due to wind, the robotic arm will not follow drastically. When the robotic arm approaches the fruit, A... img Approaching A ref The gain is released to the upper limit to ensure high-precision fine-tuning during the alignment phase.

[0027] Furthermore, the construction of smooth dynamic damping based on the singular value decomposition of the image Jacobian matrix includes: performing singular value decomposition on the current image Jacobian matrix:

[0028] I img =UΣV T ;

[0029] Extracting the minimum singular value σ min As a measure of the distance to the singular region, a high-order nonlinear dynamic damping factor is constructed:

[0030] ;

[0031] Where, σ min σ is the minimum singular value obtained by decomposing the Jacobian matrix of the current image; th The calibrated safety singularity boundary threshold; μ max The maximum penalty damping set for the system;

[0032] The solution is performed at the singular value level to generate the camera spatial velocity command vector:

[0033] ;

[0034] Among them, u i and v i These correspond to the i-th column vectors of the orthogonal matrices U and V after the Jacobian matrix SVD decomposition, respectively; σ i Γ is the i-th singular value; Γ is the feedforward compensation coefficient for the target's view plane velocity; The feedforward rate of change of the target in the image plane;

[0035] The feedforward term actively predicts the target's position in the next frame, offsetting the lag of the pure feedback closed loop and enabling the target to follow the wind disturbance ahead of time.

[0036] Furthermore, the real-time calculation of spatial alignment includes: calculating spatial alignment using a Gaussian decay mapping.

[0037] ;

[0038] Among them, (u t ,v t () represents the pixel coordinates of the center of the fruit stalk extracted from local visual perception; σ represents the reference center pixel coordinates of the end effector sleeve; the difference between the two represents the alignment deviation of the image plane; u With σ v The tolerance standard deviation parameter is set based on the physical opening size of the sleeve;

[0039] When the fruit stalk deviation is within the allowable tolerance range of the sleeve radius, the Gaussian function remains a flat high score close to 1; once the deviation exceeds σ... u σ v The set physical boundaries caused the confidence score to plummet exponentially to near zero, eliminating the risk of the robotic arm forcibly triggering shearing and damaging the fruit in a severely eccentric state.

[0040] The real-time calculation of attitude matching degree and depth approximation degree includes:

[0041] Attitude matching is achieved using a cosine decay mapping:

[0042] ;

[0043] Where, θ target θ is the tilt angle of the main axis of the current fruit stalk; ref The reference cutting angle is the angle at which the shearing blade is under optimal stress; k is the set nonlinear sensitivity index, and k≥3 is used to increase the score difference between slight tilt and severe tilt.

[0044] The depth approximation uses a logistic S-type mapping:

[0045] ;

[0046] Among them, A cur A is the effective pixel area of ​​the fruit mask in the current frame; trigger The trigger reference area is the area at which the fruit, calibrated by the system, just fills the inner cavity of the sleeve; γ is the gain coefficient for controlling the steepness of the curve.

[0047] When A cur Much smaller than A trigger The time score is strongly suppressed to an extremely low level by the dead zone effect, only when A cur Approaching A trigger The time score undergoes an S-shaped abrupt jump, and filtering out the linear ambiguity region of depth ensures the decisiveness of the shearing timing.

[0048] Furthermore, the calculation of the final shear confidence using the multiplicative fusion mechanism includes:

[0049] ;

[0050] Among them, S align Spatial alignment; S pose Let S be the pose matching degree. depth For depth approximation; S safe It is the minimum safe alignment threshold. ε is the forced bottom-line step function for spatial alignment, and ε is the minimum tolerance threshold for spatial alignment.

[0051] The multiplicative fusion mechanism has a veto power; when the confidence level of any single feature is extremely low, the total score C is affected. cut Immediate zeroing effectively prevents accidental shearing under non-ideal poses; the system sets a shearing confidence threshold T. trigger With steady time window t stable Only when C cut ≥T trigger And the duration exceeds t stable When the optimal cutting time is determined, a cutting enable signal is output to drive the end effector to complete the stem cutting.

[0052] Furthermore, the delay in acquiring the verification frame image after the shearing is completed, and the calculation of the feature retention rate, includes: a delay of Δt after the shearing action is completed. drop Time-based verification frame images are acquired, and the effective area A of the fruit category pixels before and after cutting is extracted. pre and A post Define the feature residual rate:

[0053] ;

[0054] Based on the residual rate range, the status transition is performed: successful shedding status. When the fruit stalk breaks off, record the information in the log and plan the next target; in a semi-suspended state If the fruit stalk is found to be entangled by the phloem, the robotic arm is controlled to retract slightly along the normal direction of the fruit stalk, using gravity to break off the remaining tissue, and a second measurement of the residual rate is performed; in a severely jammed state, η residual If the residual rate is ≥0.70, the shearing is deemed to have failed, triggering the secondary shearing logic of the in-situ blade opening and closing. If the residual rate still does not decrease after reaching the maximum number of attempts, an abnormal interruption is thrown and an alarm is triggered, and the system enters the physical protection lockout mode.

[0055] The advantage of this invention lies in the control method for a citrus harvesting robot based on dual-view collaboration and visual servoing. By constructing a two-layer control architecture of global coarse positioning + local fine servoing, it effectively solves the problems of positioning error accumulation, inability to cope with dynamic environments, and easy damage to fruit in existing open-loop control methods. Real-time closed-loop feedback is achieved using a local macro camera, combined with multiple mechanisms such as occlusion confidence, adaptive gain, and smooth dynamic damping, enabling dynamic tracking and automatic posture compensation of the fruit stem target. This ensures that the end effector can accurately fit the fruit and complete the non-destructive cutting. Closed-loop verification of the harvesting results is achieved through feature residue rate measurement, changing the traditional robot's blind operation mode of simply cutting without regard to success, and significantly improving the effective success rate of a single operation.

[0056] The advantages of this invention also lie in the control method for a citrus harvesting robot based on dual-view collaboration and visual servoing. By introducing occlusion confidence to establish a smooth update equation for the feature state, constructing an adaptive gain coupled with visual depth scale, and employing smooth dynamic damping based on singular value decomposition, a multi-layered visual servoing control mechanism is formed. This mechanism effectively addresses complex working conditions in unstructured orchards, such as leaf occlusion, wind disturbance, and the singular configuration of the robotic arm, ensuring the continuity, stability, and safety of the control process, and achieving advanced tracking and high-precision alignment of dynamic targets.

[0057] The advantages of this invention also lie in the control method for a citrus harvesting robot based on dual-view collaboration and visual servoing. This method calculates multidimensional confidence sub-scores using Gaussian decay mapping, cosine decay mapping, and logistic S-shaped mapping, employs a multiplicative fusion mechanism to achieve a veto-type shearing decision, and combines feature residual rate measurement to complete closed-loop verification of the harvesting results. This mechanism strictly maps the physical boundary constraints of the sleeve and the optimal shearing pose requirements, effectively filtering out deep linear ambiguity regions, eliminating the risk of mis-shearing under non-ideal poses, and achieving accurate determination of harvesting timing and intelligent recovery from abnormal states. Attached Figure Description

[0058] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0059] Figure 1 This is a schematic diagram of the control method for a citrus harvesting robot based on dual-view collaboration and visual servoing according to this application. Detailed Implementation

[0060] Embodiments of the present invention are described in detail below. Examples of these embodiments are illustrated in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0061] In the description of this application, it should be noted that, unless otherwise specified and limited, the terms "installation", "connection" and "linkage" should be interpreted broadly, and can refer to mechanical or electrical connections, or internal connections between two components, or direct connections. "Up", "down", "left", "right", etc., are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may change.

[0062] like Figure 1The diagram illustrates a control method for a citrus harvesting robot based on dual-view collaboration and visual servoing, comprising the following steps: S1: Activate a global depth camera to collect point cloud data of the fruit trees, identify the bounding boxes of mature citrus fruits based on a target detection model, and calculate the approximate three-dimensional coordinates of the fruit center in the base coordinate system using depth information. S2: Set a pre-operation point, perform inverse kinematics calculation to plan a collision-free trajectory, drive the robotic arm to move rapidly to deliver the end effector to the pre-operation point, and transfer system control to a local macro camera. S3: Extract the fruit stem feature vector based on the local macro camera, introduce occlusion confidence to establish a smooth update equation for the feature state, construct an adaptive gain coupled with visual depth scale, construct smooth dynamic damping based on the singular value decomposition of the image Jacobian matrix, and generate a camera spatial velocity command vector at the singular value level to drive the differential motion of the robotic arm to correct the end effector posture in real time. S4: Calculate the confidence sub-scores of spatial alignment, posture matching, and depth approximation in real time, use a multiplicative fusion mechanism to calculate the final shear confidence, and output a shear enable signal when the shear confidence reaches a threshold and the duration exceeds a stable time window. S5: After the cutting is completed, a delayed acquisition of verification frame images is performed, the feature residual rate is calculated, and state transition is executed based on the residual rate interval to complete closed-loop verification and anomaly scheduling. This invention effectively solves the problems of positioning error accumulation, inability to cope with dynamic environments, and easy damage to fruit in existing open-loop control methods by constructing a two-layer control architecture of global coarse positioning + local fine servo. Using a local macro camera for real-time closed-loop feedback, combined with multiple mechanisms such as occlusion confidence, adaptive gain, and smooth dynamic damping, dynamic tracking and automatic attitude compensation of the fruit stem target are achieved, ensuring that the end effector can accurately fit into the fruit and complete the non-destructive cutting. Closed-loop verification of the harvesting results is achieved through feature residual rate measurement, changing the traditional robot's blind operation mode of simply cutting without regard to success, and significantly improving the effective success rate of a single operation. The following details the above steps.

[0063] For step S1: Start the global depth camera to collect point cloud data of fruit trees, identify the bounding box of mature citrus based on the target detection model, and calculate the rough three-dimensional coordinates of the fruit center in the base coordinate system by combining the depth information.

[0064] The harvesting robot includes a main body, a robotic arm, a global depth camera, an end effector, and a local macro camera. The global depth camera and robotic arm are mounted on the main body, and the end effector and local macro camera are mounted on the end of the robotic arm. The global depth camera is used to perform global perception and coarse localization, while the local macro camera is used to perform local fine servoing.

[0065] The system activates a global RGB-D camera in a stationary state to collect point cloud data of the fruit trees. By loading an object detection model trained on a real orchard cube dataset, it identifies the bounding boxes of ripe citrus fruits. Combining depth information, it calculates the approximate 3D coordinates of the fruit's center in the robot's base coordinate system, providing target orientation for subsequent robotic arm movements.

[0066] Specifically, in the embodiments of this application, starting the global depth camera to collect fruit tree point cloud data includes: the system starts the global RGB-D camera in a stationary state, loads a target detection model trained based on a real orchard multidimensional dataset, the multidimensional dataset containing positive and negative samples of images of the fruit taken by the camera under different lighting conditions, at different distances between the camera and the fruit, and from eye level, top, and bottom perspectives, and identifies the bounding boxes of mature citrus fruits and calculates the rough three-dimensional coordinates of the fruit center in the base coordinate system. Through the stationary acquisition method and multidimensional dataset training, the stability and reliability of global localization are improved, providing accurate initial target pointing for subsequent local fine-grained servoing.

[0067] For step S2: Set a pre-work point, perform inverse kinematics calculation to plan a collision-free trajectory, drive the robotic arm to move quickly to deliver the end effector to the pre-work point, and transfer system control to the local macro camera.

[0068] Step S2 is used for path planning and rapid approach. In the embodiments of this application, setting a pre-operation point includes: setting the pre-operation point at a specific distance from the fruit surface along the line connecting the centers of the fruit. This point serves as a switching node between global coarse positioning and local fine servoing, realizing the handover of control in the dual-view collaborative architecture and switching the system from open-loop control to closed-loop visual servo control.

[0069] For step S3: Extract the fruit stem feature vector based on the local macro camera, introduce occlusion confidence to establish a smooth update equation for the feature state, construct an adaptive gain coupled with visual depth scale, construct smooth dynamic damping based on the singular value decomposition of the image Jacobian matrix, and solve at the singular value level to generate the camera spatial velocity command vector, which drives the differential motion of the robotic arm to correct the end pose in real time.

[0070] In real orchards, the end-point camera faces challenges such as high-frequency target swaying caused by wind disturbances, brief obstructions from branches and leaves, and the robotic arm easily entering a kinematic singularity zone when approaching fruit. Conventional pseudo-inverse vision servoing algorithms can lead to system divergence or cause jerks in motor acceleration under these conditions. This step addresses these issues by establishing a control law with feedforward prediction and matrix-based smooth damping.

[0071] In the embodiments of this application, extracting the fruit stalk feature vector based on a local macro camera includes: extracting the fruit stalk feature vector through semantic segmentation, wherein the feature vector includes pixel coordinates, logarithm of cross-sectional area, and principal axis tilt angle. Specifically, the feature vector s = [u, v, A] img ,θ]T, where (u, v) are pixel coordinates, ln(A img Let θ be the logarithm of the cross-sectional area, and θ be the tilt angle of the principal axis. An occlusion confidence score based on image mask integrity is introduced, with values ​​ranging from 0 to 1. Through multidimensional feature vector design and the introduction of occlusion confidence, a complete fruit stem representation system is established, providing rich feedback information for visual servo control and possessing early warning capabilities to cope with occlusion interference.

[0072] In the embodiments of this application, establishing the feature state smooth update equation includes: introducing occlusion confidence to establish a first-order Markov smooth update mechanism, and establishing the feature state smooth update equation:

[0073] ;

[0074] Where ρ is the occlusion confidence score calculated using the pixel integrity of the local visual mask, and its value ranges from [0,1]. visual This is the observation feature vector extracted by the camera in the current frame. t-1 and These represent the characteristic state and rate of change of the previous moment, respectively. Δt is the sampling period of the control system. When the fruit is momentarily blocked by leaves, ρ approaches 0, and the system relies entirely on historical states and rates of change for inertial extrapolation. When the field of vision is clear, ρ approaches 1, and the system is dominated by real-time observation, cutting off the step command for the robotic arm position caused by the momentary loss of visual features.

[0075] This formula constructs a first-order Markov smooth update mechanism. In unstructured harvesting, when fruit is momentarily obscured by leaves (ρ approaches 0), the system no longer accepts the abrupt or lost s. visual Instead of relying on historical states and rates of change for inertial extrapolation, the system is entirely driven by real-time observation when the field of view is clear (ρ approaches 1). This fundamentally eliminates the step command for the robotic arm's position caused by the instantaneous loss of visual features.

[0076] In the embodiments of this application, constructing the adaptive gain of visual depth scale coupling includes:

[0077] By implicitly representing the real physical distance using the cross-sectional area of ​​the fruit stalk, a dynamic gain coefficient is constructed:

[0078] ;

[0079] Where, λ min With λ maxThese represent the minimum and maximum servo gains allowed by the system, respectively. α is the attenuation slope coefficient. e is the deviation between the current image feature deviation and the desired feature, where ||e|| is the Euclidean norm of the deviation between the current image feature deviation and the desired feature. A img Let A be the current characteristic cross-sectional area of ​​the fruit stalk. ref Let A be the target's reference cross-sectional area. When the camera is far from the target, A... img The gain is extremely small, and the system forcibly reduces the overall gain. Even if the target experiences a large pixel deviation due to wind, the robotic arm will not follow drastically. When the robotic arm approaches the fruit, A... img Approaching A ref The gain is released to its upper limit to ensure high-precision fine-tuning during the alignment phase. By introducing the area ratio term, adaptive adjustment is achieved for low-gain jitter reduction at long distances and high-gain precision maintenance at close distances, ensuring the safety of long-distance tracking and the ability to perform high-precision fine-tuning during the alignment phase.

[0080] Specifically, the square of the area ratio term A is introduced. img / Ar ef As an implicit representation of physical distance. When the camera is far from the target (A img (Very small), the system forcibly reduces the overall gain λ. At this time, even if the target has a huge pixel deviation ||e|| in the picture due to wind, the robotic arm will not produce violent large-range following movements, ensuring the safety of long-distance tracking; when the robotic arm approaches the fruit (A img Approaching A ref When the gain is released to its upper limit, it ensures high-precision fine-tuning during the alignment phase.

[0081] In the embodiments of this application, constructing smooth dynamic damping based on the singular value decomposition of the image Jacobian matrix includes: performing singular value decomposition on the current image Jacobian matrix:

[0082] I img =UΣV T ;

[0083] Extracting the minimum singular value σ min As a measure of the distance to the singular region, to ensure the kinematic C when the robotic arm crosses the singular boundary 1 Continuity (continuity of the first derivative), constructing higher-order nonlinear dynamic damping factors:

[0084] ;

[0085] Where, σ min It is the smallest singular value obtained by decomposing the Jacobian matrix of the current image. This is the calibrated safety singularity boundary threshold. μ maxThe maximum penalty damping is set for the system. Specifically, the minimum singular value directly reflects the degree to which the robotic arm approximates a singular configuration (where the motion capabilities of each joint are limited). A high-order polynomial is used instead of a piecewise constant to ensure the damping factor. In crossing σ th The first derivative is continuous at the boundary. The physical significance of this mathematical property is that it ensures that the angular velocity command changes of the motors driving each joint of the robotic arm are smooth, avoiding the sudden acceleration changes (Jerk) that occur when entering the singular region, and protecting the end effector and reducer from mechanical impact damage.

[0086] The solution is performed at the singular value level to generate the camera spatial velocity command vector:

[0087] ;

[0088] Among them, u i and v i These correspond to the i-th column vectors of the orthogonal matrices U and V after the Jacobian matrix SVD decomposition, respectively. Let i be the i-th singular value. The feedforward compensation coefficient is the velocity in the target's view plane. The feedforward rate of change of the target in the image plane. The feedforward term actively predicts the target's position in the next frame, offsetting the lag of the pure feedback closed loop and achieving advanced tracking of the target amidst wind disturbance. The system uses this velocity vector V... cam Real-time drive of the robotic arm.

[0089] By constructing dynamic damping at the SVD level and introducing feedforward terms, the kinematic continuity of the robotic arm when crossing singular boundaries is ensured, enabling advanced tracking of wind-induced targets and effectively suppressing pure feedback closed-loop lag and mechanical shock. Unlike directly using a black-box pseudo-inverse matrix, this underlying expansion achieves decoupling of the control dimension. It only applies strong damping suppression to minimal singular components approaching zero (the root cause of divergent runaway), while preserving the full-dimensional response in other non-singular directions. Simultaneously, the superimposed Γ· The feedforward term actively predicts the target's position in the next frame, offsetting the lag in the pure feedback closed loop caused by the camera frame rate and system computation time, thus achieving advanced tracking of wind-disrupted targets.

[0090] For step S4: Real-time calculation of confidence sub-scores for spatial alignment, pose matching and depth approximation, a multiplicative fusion mechanism is used to calculate the final shear confidence. When the shear confidence reaches the threshold and the duration exceeds the stable time window, a shear enable signal is output.

[0091] In the embodiments of this application, real-time calculation of spatial alignment includes: calculating spatial alignment using a Gaussian decay map.

[0092] ;

[0093] Among them, (u t ,v t (u) represents the pixel coordinates of the center of the fruit stalk extracted from local visual perception. e ,v e () represents the reference center pixel coordinates of the end effector sleeve. The difference between the two represents the alignment deviation of the image plane. With σ v This is the standard deviation parameter for tolerance, set based on the physical opening size of the sleeve.

[0094] When the fruit stalk deviation is within the allowable tolerance range of the sleeve radius, the Gaussian function remains a flat high score close to 1; once the deviation exceeds σ... u σ v The set physical boundaries cause the confidence score to plummet exponentially to near zero, eliminating the risk of the robotic arm forcibly triggering shearing and damaging the fruit in a severely eccentric state.

[0095] A Gaussian model is used instead of a simple linear proportional model because the physical boundaries of the sleeve are a hard constraint. When the fruit stem deviation is within the allowable tolerance range of the sleeve radius (i.e., close to the center), the Gaussian function maintains a gentle high score close to 1; once the deviation exceeds... The confidence score will drop exponentially to near zero when the physical boundary is set. By using Gaussian decay mapping, a soft constraint on the physical boundary of the sleeve is achieved, maintaining a high score within the tolerance range and dropping sharply when the boundary is exceeded. This strictly maps the physical condition that "it is absolutely impossible to fit the sleeve if it deviates from the edge," eliminating the risk of forced shearing under severe eccentricity.

[0096] Real-time calculation of attitude matching degree includes:

[0097] Attitude matching is achieved using a cosine decay mapping:

[0098] ;

[0099] Where, θ target θ is the tilt angle of the main axis of the current fruit stalk. ref The reference cutting angle is the angle at which the shearing blade is under optimal stress. k is a set nonlinear sensitivity index, and k≥3 is used to increase the score difference between slight tilt and severe tilt.

[0100] The cosine function naturally possesses the mathematical property of mapping angular deviations to the [0,1] interval. A score of 1 is given when the fruit stalk is parallel to the blade (angle difference is 0). The purpose of introducing the higher-order exponent k is to widen the score gap between slight and severe tilting. In unstructured orchards, if the fruit stalk tilt angle is too large, the blade is prone to slippage or jamming during closing, leading to motor overload. This nonlinear decay model ensures that the system only assigns high confidence when the mechanical structure is in the optimal shearing posture.

[0101] Real-time calculation of depth approximation includes:

[0102] The depth approximation uses a logistic S-type mapping:

[0103] ;

[0104] Among them, A cur A represents the effective pixel area of ​​the fruit mask in the current frame. trigger The trigger reference area is the area at which the calibrated fruit exactly fills the inner cavity of the sleeve. γ is the gain coefficient controlling the steepness of the curve. When A cur Much smaller than A trigger The time score is strongly suppressed to an extremely low level by the dead zone effect, only when A cur Approaching A trigger The time score undergoes an S-shaped abrupt jump, and filtering out the linear ambiguity region of depth ensures the decisiveness of the shearing timing.

[0105] Specifically, in monocular macro photography, relying on linear area to determine depth is highly susceptible to interference from differences in fruit size (large fruits in the distance and small fruits nearby may have the same area in the image). A logistic sigmoid function is used, such that when A... cur Much smaller than A trigger (i.e., during the approach process), the score is strongly suppressed to an extremely low level by the dead zone effect, shielding it from the linear accumulation interference of long-distance features and background noise; only when the end is extremely close to the fruit, A cur Approaching or even surpassing A trigger Only when this happens will the score experience an S-shaped abrupt jump. This model effectively filters out deep linear ambiguity, ensuring the decisiveness and accuracy of the shearing timing.

[0106] Finally, the multiplicative fusion mechanism is used to calculate the final shear confidence score, including:

[0107] ;

[0108] Among them, S align For spatial alignment. S pose Let S be the pose matching degree. depth For depth approximation, S safeε is the minimum safe alignment threshold, which is the triggering critical point of the step function. H(x) is the mandatory bottom-line step function for spatial alignment, and ε is the minimum tolerance threshold for spatial alignment.

[0109] The multiplicative fusion mechanism has a veto power; when the confidence level of any single feature is extremely low, the total score C is affected. cut Immediate zeroing effectively prevents accidental shearing under non-ideal poses. The system sets a shearing confidence threshold T. trigger With steady time window t stable Only when C cut ≥T trigger And the duration exceeds t stable When the optimal shearing time is determined, a shearing enable signal is output to drive the end effector to complete the stem shearing. Through a multiplicative veto mechanism and a dual-condition triggering design, erroneous shearing under non-ideal poses is effectively prevented, ensuring the decisiveness and reliability of the shearing action and achieving accurate determination of the optimal shearing time.

[0110] For step S5: After the shearing is completed, delay the acquisition of the verification frame image, calculate the feature residual rate, and perform state transition according to the residual rate interval to complete the closed-loop verification and anomaly scheduling.

[0111] In the embodiments of this application, the verification frame image is acquired after the shearing is completed, and the feature retention rate is calculated by delaying the acquisition by Δt after the shearing action is completed. drop Time-based verification frame images are acquired, and the effective area A of the fruit category pixels before and after cutting is extracted. pre and A post Define the feature residual rate:

[0112] ;

[0113] Execute state transition based on residual rate range:

[0114] Successful shedding state η residual If the value is ≤0.15, the fruit stalk is determined to be broken off, the log is recorded, and the next target is planned.

[0115] In semi-suspended state, 0.15 < η residual When the value is less than 0.70, the downward displacement component Δd of the target center is used to determine that the fruit stalk is affected by the phloem. The robotic arm is then controlled to retract slightly along the normal direction of the fruit stalk to break off the remaining tissue by gravity, and the residual rate is measured a second time.

[0116] Assume that in the critical frame image before cropping, the pixel coordinates of the fruit center are P. pre (u pre , v pre In the cropped verification frame image, the pixel coordinates of the fruit center are P. pos (u post , vpost Then the displacement vector is defined as:

[0117] ;

[0118] Δd not only includes the distance moved, but more importantly, it includes the direction of movement. If only the absolute distance between the two center points is calculated (for example, calculating that the fruit moved 2 centimeters), the system cannot determine how this displacement occurred. It could be because the robotic arm accidentally bumped into the fruit while retracting, causing it to shift to the left or right, or it could be due to being blown by the wind. A simple "distance" cannot serve as a valid basis for determining "semi-suspension."

[0119] The reason for emphasizing the "downward displacement component" is that, in the context of the real physical world, when the xylem of a fruit stalk is cut off, leaving only the flexible phloem (bark fibers) connecting it, the fruit will experience a slight downward fall under the influence of gravity, and then be suspended by the phloem. Therefore, the algorithm only cares about the component that follows the direction of gravity (assuming the positive direction of the Y-axis in the image coordinate system is downward, i.e., only considering Δv).

[0120] Only when the downward component Δv is greater than the set gravity loosening threshold, and the overall characteristic residual rate η residual When the fruit is still very high (proving that it is still in the frame and has not fallen), an AND operation between these two conditions is needed to accurately pinpoint that the fruit is in a "semi-suspended state with the phloem attached".

[0121] Severe stuck state η residual If the residual rate is ≥0.70, the shearing is deemed to have failed, triggering the secondary shearing logic of the in-situ blade opening and closing. If the residual rate still does not decrease after reaching the maximum number of attempts, an abnormal interruption is thrown and an alarm is triggered, and the system enters the physical protection lockout mode.

[0122] By quantitatively measuring the residual rate and dividing the state transitions into intervals, the disappearance of the target is transformed into a quantifiable mathematical indicator, which drives the finite state machine to perform intelligent anomaly recovery. This changes the traditional blind operation mode of robots and realizes closed-loop verification of harvesting results and adaptive anomaly handling.

[0123] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the above embodiments do not limit the present invention in any way, and all technical solutions obtained by equivalent substitution or equivalent transformation fall within the protection scope of the present invention.

Claims

1. A control method for a citrus harvesting robot based on dual-view collaboration and visual servoing, characterized in that, Includes the following steps: The global depth camera is activated to collect point cloud data of fruit trees. The bounding box of mature citrus fruits is identified based on the target detection model. The rough three-dimensional coordinates of the fruit center in the base coordinate system are calculated by combining the depth information. Set a pre-operation point, perform inverse kinematics calculation to plan a collision-free trajectory, drive the robotic arm to move quickly to deliver the end effector to the pre-operation point, and transfer system control to the local macro camera. Based on the extraction of fruit stem feature vectors by local macro camera, the occlusion confidence is introduced to establish a smooth update equation for feature state, an adaptive gain coupled with visual depth scale is constructed, and smooth dynamic damping is constructed based on the singular value decomposition of the image Jacobian matrix. The solution is carried out at the singular value level to generate camera spatial velocity command vector, which drives the differential motion of the robotic arm to correct the end posture in real time. The confidence sub-scores of spatial alignment, pose matching and depth approximation are calculated in real time. The final shear confidence is calculated using a multiplicative fusion mechanism. When the shear confidence reaches the threshold and the duration exceeds the stable time window, the shear enable signal is output. After the cutting is completed, the verification frame image is acquired after a delay, the feature residual rate is calculated, and the state transition is performed according to the residual rate interval to complete the closed-loop verification and anomaly scheduling. The real-time calculation of spatial alignment includes: calculating spatial alignment using a Gaussian decay mapping. ； Among them, (u t ,v t (u) represents the pixel coordinates of the center of the fruit stalk extracted from local vision; e ,v e ) represents the reference center pixel coordinates of the end effector sleeve; the difference between the two represents the alignment deviation of the image plane; σ u With σ v The tolerance standard deviation parameter is set based on the physical opening size of the sleeve; The real-time calculation of attitude matching degree and depth approximation degree includes: Attitude matching is achieved using a cosine decay mapping: ； Where, θ target θ is the tilt angle of the main axis of the current fruit stalk; ref The reference cutting angle is the angle at which the shearing blade is under optimal stress; k is the set nonlinear sensitivity index, and k≥3 is used to increase the score difference between slight tilt and severe tilt. The depth approximation uses a logistic S-type mapping: ； Among them, A cur A is the effective pixel area of ​​the fruit mask in the current frame; trigger The trigger reference area is the area at which the fruit, calibrated by the system, just fills the inner cavity of the sleeve; γ is the gain coefficient for controlling the steepness of the curve. The calculation of the final shear confidence using the multiplicative fusion mechanism includes: ； Among them, S align Spatial alignment; S pose Let S be the pose matching degree. depth For depth approximation; S safe H is the minimum safe alignment threshold, H is the forced bottom-line step function for spatial alignment, and ε is the minimum tolerance threshold for spatial alignment. The multiplicative fusion mechanism has a veto power; when the confidence level of any single feature is extremely low, the total score C is affected. cut Immediate zeroing effectively prevents accidental shearing under non-ideal poses; the system sets a shearing confidence threshold T. trigger With steady time window t stable Only when C cut ≥T trigger And the duration exceeds t stable When the optimal cutting time is determined, a cutting enable signal is output to drive the end effector to complete the stem cutting.

2. The control method for a citrus harvesting robot based on dual-view collaboration and visual servoing according to claim 1, characterized in that, The process of activating the global depth camera to collect fruit tree point cloud data includes: activating the global RGB-D camera in a stationary state, loading a target detection model trained on a real orchard multidimensional dataset, which contains positive and negative samples of images of the fruit taken by the camera under different lighting conditions, at different distances between the camera and the fruit, and from the perspectives of eye level, top view, and bottom view. The system identifies the bounding box of the mature citrus fruit and calculates the rough three-dimensional coordinates of the fruit center in the base coordinate system.

3. The control method for a citrus harvesting robot based on dual-view collaboration and visual servoing according to claim 1, characterized in that, The setting of the preparatory work point includes: the preparatory work point is located at a specific distance from the fruit surface along the line connecting the centers of the fruit. This point serves as the switching node between global coarse positioning and local fine servo, realizing the handover of control in the dual-view collaborative architecture and switching the system from open-loop control to closed-loop visual servo control.

4. The control method for a citrus harvesting robot based on dual-view collaboration and visual servoing according to claim 1, characterized in that, The extraction of fruit stalk feature vector based on local macro camera includes: extracting fruit stalk feature vector through semantic segmentation, wherein the feature vector includes pixel coordinates, logarithm of cross-sectional area, and principal axis tilt angle; and introducing occlusion confidence based on image mask integrity, wherein the occlusion confidence value ranges from 0 to 1.

5. The control method for a citrus harvesting robot based on dual-view collaboration and visual servoing according to claim 4, characterized in that, The establishment of the feature state smooth update equation includes: introducing occlusion confidence to establish a first-order Markov smooth update mechanism, and establishing the feature state smooth update equation: ； Where ρ is the occlusion confidence score calculated from the pixel integrity of the local visual mask, and its value range is [0,1]; s visual The observed feature vector extracted by the camera in the current frame; s t-1 and These represent the characteristic state and characteristic rate of change of the previous moment, respectively; Δt is the sampling period of the control system. When the fruit is momentarily obscured by leaves, ρ approaches 0, and the system relies entirely on historical states and rates of change for inertial extrapolation; when the field of vision is clear, ρ approaches 1, and the system is dominated by real-time observation, cutting off the robotic arm position step commands caused by the momentary loss of visual features.

6. The control method for a citrus harvesting robot based on dual-view collaboration and visual servoing according to claim 1, characterized in that, The adaptive gain for constructing visual depth-scale coupling includes: By implicitly representing the real physical distance using the cross-sectional area of ​​the fruit stalk, a dynamic gain coefficient is constructed: ； Where, λ min With λ max Here, represents the minimum and maximum servo gain allowed by the system, respectively; α is the attenuation slope coefficient; e is the deviation between the current image feature deviation and the desired feature, and ||e|| is the Euclidean norm of the deviation between the current image feature deviation and the desired feature; A img Let A be the current characteristic cross-sectional area of ​​the fruit stalk. ref The target reference cross-sectional area; When the camera is far from the target, A img The gain is extremely small, and the system forcibly reduces the overall gain. Even if the target experiences a large pixel deviation due to wind, the robotic arm will not follow drastically. When the robotic arm approaches the fruit, A... img Approaching A ref The gain is released to the upper limit to ensure high-precision fine-tuning during the alignment phase.

7. The control method for a citrus harvesting robot based on dual-view collaboration and visual servoing according to claim 1, characterized in that, The construction of smooth dynamic damping based on the singular value decomposition of the image Jacobian matrix includes: performing singular value decomposition on the current image Jacobian matrix. I img =UΣV T ; Extracting the minimum singular value σ min As a measure of the distance to the singular region, a high-order nonlinear dynamic damping factor is constructed: ； Where, σ min σ is the minimum singular value obtained by decomposing the Jacobian matrix of the current image; th The calibrated safety singularity boundary threshold; μ max The maximum penalty damping set for the system; The solution is performed at the singular value level to generate the camera spatial velocity command vector: ； Among them, u i and v i These correspond to the i-th column vectors of the orthogonal matrices U and V after the Jacobian matrix SVD decomposition, respectively; σ i Γ is the i-th singular value; Γ is the feedforward compensation coefficient for the target's view plane velocity; The feedforward rate of change of the target in the image plane; The feedforward term actively predicts the target's position in the next frame, offsetting the lag of the pure feedback closed loop and enabling the target to follow the wind disturbance ahead of time.

8. The control method for a citrus harvesting robot based on dual-view collaboration and visual servoing according to claim 1, characterized in that, The calculation of the feature retention rate includes a delay of Δt after the shearing action is completed, which is used to acquire and verify the frame image. drop Time-based verification frame images are acquired, and the effective area A of the fruit category pixels before and after cutting is extracted. pre and A post Define the feature residual rate: or residual =A post / A pre ; Execute state transition based on residual rate range: Successful shedding state η residual If η is ≤0.15, the fruit stalk is considered broken; the log is recorded and the next target is planned; in the semi-suspended state, η <0.

15. residual When the value is less than 0.70, the fruit stalk is determined to be entangled by the phloem. The robotic arm is controlled to retract slightly along the normal direction of the fruit stalk, using gravity to break off the remaining tissue, and the residual rate is measured a second time; in a severely stuck state, η residual If the residual rate is ≥0.70, the shearing is deemed to have failed, triggering the secondary shearing logic of the in-situ blade opening and closing. If the residual rate still does not decrease after reaching the maximum number of attempts, an abnormal interruption is thrown and an alarm is triggered, and the system enters the physical protection lockout mode.

Images