An intelligent control system and method for spatial clamping posture of an orchard picking robot

Abstract

The present application relates to the technical field of agricultural robot control, in particular to a space clamping posture intelligent control system and method for orchard picking robot, the present application obtains scene point cloud through binocular vision, segmenting and connecting point cloud cluster and calculating rough axial vector, obtaining initial clamping posture, after the end effector enters the near field, triggering the wrist structured light unit to project the coded light spot, collecting the non-lambertian reflectance light band image sequence, according to the space-time deformation characteristics of the light band, the local three-dimensional profile of the connecting part is inversed, the spatial axis vector is fitted and the confidence probability is generated, the posture arbitration decision unit takes the confidence probability as the weight, within the preset decision time limit, the far field preliminary adjustment amount and the near field correction amount are weighted and fused or optimally decided, the joint adjustment angle instruction is generated, the adjustment execution control unit is based on the robot dynamics model, the predictive control algorithm is used to optimize the joint angle control sequence in reverse, driving the pitch and yaw joint movement.

Description

Technical Field

[0001] This invention relates to the field of agricultural robot control technology, and more specifically, to an intelligent control system and method for the spatial gripping posture of an orchard harvesting robot. Background Technology

[0002] Large-scale fruit tree planting has driven the development of automated orchard harvesting. Harvesting robots rely on robotic arms to grip fruit stems to complete the harvest. Whether the end gripping posture is perpendicular to the fruit stem axis directly determines the success rate of fruit harvesting, which is a technical challenge for intelligent harvesting. Most existing commercial harvesting robots use a single binocular vision system to locate the fruit stem. However, open-air orchards present challenges such as drastic changes in natural light, overlapping branches and leaves, and differences in the thickness and shape of fruit stems. The point cloud data collected by binocular vision is mixed with a large number of noise points, and the fitting accuracy of the cylindrical fruit stem axis is limited, only outputting coarse posture parameters. The gripper's working plane is difficult to be perpendicular to the fruit stem, which easily leads to malfunctions such as crushing fruit peels, breaking fruit branches, and fruit falling off during harvesting. Some models have added near-field structures. While traditional methods employ light detection, they generally utilize single-source data control logic. However, reflections from non-Lambertian surfaces of the fruit stalk and stray light from the forest can cause distortion in structured light imaging. Furthermore, there is no backup positioning reference after the fruit stalk axis calculation fails, and there is a lack of data reliability assessment and dual-source attitude fusion arbitration mechanisms. In addition, traditional robotic arms often use conventional closed-loop control for pitch and yaw joints without relying on propriodynamics for motion prediction. Angle adjustments are prone to overshoot and mechanical oscillations, and the measurement and control ends lack hierarchical filtering processing, resulting in frequent jitter in positioning data and drive commands. This makes it difficult to adapt to complex orchard operation environments in the field. To address this issue, we provide an intelligent control system and method for the spatial gripping posture of an orchard picking robot. Summary of the Invention

[0003] The purpose of this invention is to provide an intelligent control system and method for the spatial gripping posture of an orchard picking robot, so as to solve the problems mentioned in the background art.

[0004] To achieve the above objectives, a spatial gripping posture intelligent control system for an orchard harvesting robot is provided. The robotic arm has at least a wrist, an end effector, a pitch joint, and a yaw joint, and also includes: The far-field vision coarse localization unit is used to acquire scene point clouds of the target object and its connecting parts through a binocular vision module when the distance between the end effector and the target object exceeds the activation threshold, and to calculate the initial clamping posture of the end effector based on the scene point clouds. A near-field structured light measurement unit is fixed to the wrist of the robotic arm and is triggered when the end effector enters the near-field region within an activation threshold of the target object, including: A light spot projector is used to project a structured light spot with an coded pattern onto the connection area of ​​the target object; A high-speed macro camera is used to acquire image sequences of non-Lambertian reflective light bands formed by the structured light spot on the surface of the connection part. The light strip analysis module is used to calculate the spatial axis vector of the connecting part based on the spatiotemporal deformation characteristics in the non-Lambertian reflective light strip image sequence. The attitude arbitration decision unit receives the initial clamping attitude and the spatial axis vector respectively, calculates the attitude deviation, and uses the confidence probability generated by the light strip analysis module in the solution as the weight to adjudicate and generate the adjustment angle command of the pitch joint and yaw joint within the preset decision time limit. In the final stage before the end effector performs the target action, the adjustment control unit drives the pitch joint and yaw joint according to the adjustment angle command, so that the working plane of the end effector and the spatial axis vector satisfy the preset perpendicularity relationship, thereby completing the closed-loop intelligent control of the spatial clamping posture.

[0005] The second objective of this invention is to provide a method for implementing an intelligent control system for the spatial gripping posture of an orchard harvesting robot, comprising the following steps: S1. When the distance between the end effector and the target exceeds the activation threshold, the scene point cloud containing the target and the connecting part is acquired through the binocular vision module. The scene point cloud is segmented to obtain the connecting part point cloud cluster. The main direction vector of the connecting part point cloud cluster is calculated as the coarse estimation axis vector. Based on the coarse estimation axis vector, the initial adjustment amount of the pitch joint and yaw joint that makes the preset standard working plane normal vector of the end effector perpendicular to it is calculated. The spatial attitude of the end effector corresponding to the initial adjustment amount is determined as the initial clamping attitude. S2. When the end effector enters the near-field region within the activation threshold of the target object, a measurement is triggered. A structured light spot with an coded pattern is projected onto the connection region of the target object by a light spot projector fixed to the mechanical wrist. A high-speed macro camera acquires an image sequence of the non-Lambertian reflective light band formed by the structured light spot on the surface of the connection. The center line of the light band in each frame is extracted according to the preliminary adjustment amount, and the displacement and deformation of the center line of the light band are tracked between consecutive frames. Based on the continuous deformation trajectory of the center line of the light band, the local three-dimensional contour line of the surface of the connection in the light band illumination area is reconstructed in reverse through the triangulation principle. The local three-dimensional contour line is fitted with a spatial straight line to obtain the spatial axis vector of the connection, and a confidence probability characterizing the reliability of the spatial axis vector solution result is generated simultaneously. S3. Receive the initial clamping posture and its corresponding preliminary adjustment amount, as well as the spatial axis vector and its corresponding confidence probability. Transform the spatial axis vector into the robot's base coordinate system and calculate the joint angle correction amount required to make the normal vector of the end effector's working plane perpendicular to the vector. Calculate the posture deviation between the preliminary adjustment amount and the joint angle correction amount. Using the confidence probability as a weight, within a preset decision time limit, perform weighted fusion and adjudication on the adjustment tendency based on the preliminary adjustment amount and the adjustment tendency based on the joint angle correction amount to generate the final adjustment angle commands for the pitch and yaw joints. S4. In the final stage before the end effector executes the target action, the current actual angles of the pitch and yaw joints are read according to the adjustment angle command, the difference between the angles and the target angles is calculated, and a predictive control algorithm is used based on the manipulator dynamics model to predict the motion state in subsequent control cycles using the difference as input. The joint angle control sequence is then generated by reverse optimization, and the joint angle control sequence is converted into a drive signal to drive the pitch and yaw joints to move, so that the working plane of the end effector and the spatial axis vector satisfy the preset perpendicularity relationship, thus completing the closed-loop intelligent control of the spatial clamping posture.

[0006] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention acquires point cloud clusters of the connecting part through far-field binocular vision coarse localization and calculates coarsely estimated axis vectors to generate an initial gripping posture. After the end effector enters the near-field region, it triggers the structured light unit fixed to the wrist to project coded light spots. A high-speed macro camera is used to acquire light strip image sequences, analyze the spatiotemporal deformation characteristics of the light strip, and invert the local three-dimensional contour of the connecting part. A high-precision spatial axis vector is fitted and confidence probabilities are output simultaneously. The posture arbitration decision unit uses the confidence probabilities as weights and weights and fuses or selects the best option between the far-field coarse adjustment and the near-field correction within a preset decision time limit to generate the final adjustment angle command. The adjustment execution control unit is based on the manipulator dynamics model and uses a predictive control algorithm to back-optimize the joint angle control sequence, driving the pitch and yaw joints to complete smooth adjustment, so that the working plane of the end effector is perpendicular to the axis of the connecting part. This solves the problems of insufficient far-field coarse localization accuracy, lack of in-situ geometric measurement in the near field, and inability to fuse and arbitrate dual-source data, thus improving the spatial gripping posture control accuracy and operational reliability of the picking robot. Attached Figure Description

[0007] Figure 1 This is an overall system block diagram of the present invention; Figure 2 This is the overall flowchart of the present invention. Detailed Implementation

[0008] 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.

[0009] This invention provides an intelligent control system for the spatial gripping posture of an orchard harvesting robot. Please refer to [link / reference]. Figure 1 As shown, the robotic arm has at least a wrist, an end effector, a pitch joint, and a yaw joint, and also includes: The far-field vision coarse localization unit is used to acquire scene point clouds of the target object and its connecting parts through the binocular vision module when the distance between the end effector and the target object exceeds the activation threshold, and to calculate the initial clamping posture of the end effector based on the scene point cloud. The near-field structured light measurement unit is fixed to the wrist of the robotic arm and is triggered when the end effector enters the near-field region within the target activation threshold, including: A beam projector used to project a structured beam with a coded pattern onto the connection area of ​​a target object; A high-speed macro camera is used to capture image sequences of non-Lambertian reflected light bands formed by structured light spots on the surface of the joint. The light strip analysis module is used to calculate the spatial axis vector of the connection part based on the spatiotemporal deformation characteristics in the non-Lambertian reflective light strip image sequence. The attitude arbitration decision unit receives the initial clamping attitude and the spatial axis vector respectively, calculates the attitude deviation, and uses the confidence probability generated by the light strip analysis module in the solution as the weight to adjudicate and generate the adjustment angle command of the pitch joint and yaw joint within the preset decision time limit. In the final stage before the end effector performs the target action, the adjustment control unit drives the pitch and yaw joints according to the adjustment angle command, so that the working plane of the end effector meets the preset perpendicularity relationship with the spatial axis vector, thus completing the closed-loop intelligent control of the spatial clamping posture.

[0010] After acquiring complete scene point cloud data using the binocular vision module, the far-field vision coarse localization unit initiates point cloud segmentation and initial clamping posture calculation. Specifically, it uses a hierarchical clustering segmentation combined with geometric feature filtering to split different entity point clouds. The axial parameters of the fruit stalk, i.e., the connecting part, are pre-calibrated from a spatial geometric dimension to generate a coarse-adjustment attitude reference for the robot's end effector. This shortens the attitude correction range for subsequent near-field structured light measurements and reduces mechanical vibration caused by large-scale rotation of the pitch and yaw joints. It also provides a reasonable mechanical adjustment margin for the subsequent S2 step of near-field structured light acquisition of light band images and calculation of the connecting part's spatial axis vector. The scene point cloud is a set of three-dimensional discrete coordinates obtained by the binocular vision module through parallax conversion between the left and right cameras. The set contains all three-dimensional spatial point data corresponding to the fruit tree branches and leaves, the target object, and the connecting part of the target object. The target object refers to the fruit to be picked, and the connecting part refers to... The cylindrical fruit stalk structure connects the fruit and branches. The binocular vision module is equipped with a two-layer point cloud segmentation algorithm based on spatial Euclidean distance combined with local curvature features. The first layer of segmentation performs Euclidean clustering. Euclidean distance is the straight-line length between two three-dimensional coordinate points in space. A fixed Euclidean distance threshold is set in advance through the actual orchard. The discrete points in the entire scene point cloud are traversed, and continuous points with spatial spacing less than the Euclidean distance threshold are divided into the same temporary cluster. This separates three basic clusters: fruit tree background branch and leaf cluster, target object cluster, and candidate cluster for the connecting part. After completing the first coarse segmentation, the second layer of geometric feature selection is performed. The connecting part is a slender cylindrical structure, and the target object is a near-spherical fruit structure. The two types of objects have different spatial geometric features. The mean local curvature of all point clouds in each temporary cluster and the length-to-diameter ratio parameter of the cluster are extracted one by one. The length-to-diameter ratio is calculated as follows: The longest side of the cluster bounding box is divided by the shortest side. The device pre-calibrates the aspect ratio ranges corresponding to cylindrical connecting parts and spherical target objects by sampling and calibrating multiple types of fruit trees. Candidate clusters are classified based on the mean curvature and aspect ratio. Invalid point cloud clusters corresponding to branches and leaves are removed. Finally, the target object point cloud cluster and the connecting part point cloud cluster are separated from the original scene point cloud. After extracting the two types of point cloud clusters, the main direction vector of the connecting part point cloud cluster is solved. The coarse axis vector is the spatial approximate direction parameter of the cylindrical axis of the fruit stalk connection from the far-field perspective. The principal direction vector, after normalization, is directly used as the coarse axis vector of the connection. Before solving, the point cloud cluster of the connection is subjected to spatial voxelization downsampling. Voxelization downsampling divides the three-dimensional space into uniformly sized cubic meshes. Only one center point is retained within each cubic mesh, replacing all original point clouds within the mesh, to reduce redundant sampling points and lower the computational load of subsequent iterations. Then, a random sampling consistency method is used to iteratively fit a pre-defined cylindrical model. This pre-defined cylindrical model is a standard spatial geometric model used to characterize the cylindrical shape of the fruit stalk. Each iteration involves sampling from the downsampled point cloud cluster of the connection. A fixed number of points are randomly selected to form a subset of the point cloud. The spatial direction of the cylindrical axis corresponding to the current iteration is calculated based on the 3D coordinates of this subset. Then, the perpendicular distance from each of the remaining point cloud points in the connecting section to this cylindrical axis is calculated and compared to a pre-set distance threshold. Points with distances less than the threshold are marked as interior points, and those with distances exceeding the threshold are marked as exterior points. After each iteration, the total number of interior points corresponding to the current iteration is counted. This process of random sampling and model fitting iteration is repeated multiple times. After all iterations are completed, the cylindrical model with the largest number of interior points is selected. The axis direction vector inherent in this model is extracted, and then the direction vector is normalized. The normalization calculation method is as follows: Each coordinate component of the vector is divided by its own magnitude, which is the square root of the sum of the squares of the three coordinate components. The normalized vector is the principal direction vector of the point cloud cluster of the connecting part. This principal direction vector is then directly determined as the coarse-estimated axis vector of the connecting part. After the coarse-estimated axis vector is generated, the pre-stored parameters of the preset standard working plane are called. The preset standard working plane is a theoretical clamping working plane set during the robot's factory calibration stage, with the center of the end effector gripper as the reference. This plane is equipped with a fixed plane normal vector, which is a three-dimensional spatial vector perpendicular to the preset standard working plane. The equipment's preset clamping constraint requires that the standard working plane of the end effector be perpendicular to the fruit handle axis. The plane normal vector and the fruit stem axis vector are perpendicular to each other. The mathematical condition for perpendicularity is that the dot product of the two vectors equals zero. The binocular vision module uses this constraint as the optimization target to perform alignment calculations to solve for the initial adjustment amount. The alignment calculation relies on the calibrated joint motion forward and inverse kinematics models of the robot arm. The forward model is used to solve the end-effector spatial pose by the joint rotation angles, and the inverse model is used to calculate the required rotation angles of each joint from the target end-effector pose. The module first substitutes the coarsely estimated axis vector and the normal vector of the preset standard working plane into the perpendicular constraint condition to solve for the end-effector target spatial pose that satisfies the dot product equal to zero. Then, through the robot arm inverse kinematics conversion formula, the target end-effector pose is decomposed into the required rotation angle values ​​of the pitch joint and the yaw joint. The two sets of angle values ​​to be rotated are collectively referred to as the preliminary adjustment amount. The preliminary adjustment amount is the joint coarse adjustment parameter obtained from the far-field coarse positioning process. The overall spatial attitude of the end effector corresponding to the preliminary adjustment amount is directly defined as the initial clamping attitude. After the initial clamping attitude is stored, it is completely sent to the subsequent attitude arbitration decision unit as reference data via the data transmission link. After receiving the spatial axis vector and the corresponding confidence probability output by the near-field structured light unit in step S3, the attitude arbitration decision unit compares the preliminary adjustment amount corresponding to the initial clamping attitude with the joint angle correction amount solved based on the axis, calculates the attitude deviation between the two types of parameters, and generates the final adjustment angle command based on the confidence probability weighted decision. The data is then sent to the adjustment and execution control unit to drive the pitch and yaw joints. For example, in an apple picking scenario, the scene point cloud collected by binoculars is divided into apple fruit point cloud clusters and stem point cloud clusters through double-layer segmentation. After downsampling, the optimal cylindrical axis is selected through random sampling consistency iteration to obtain a coarsely estimated axis vector. This vector is then substituted into the vertical constraint to back-calculate the initial adjustment amount of 11.2 degrees for pitch and 7.3 degrees for yaw. The gripper spatial attitude corresponding to this parameter is determined as the initial gripping attitude and then transferred to subsequent arbitration calculations. The entire set of calculations from point cloud splitting to initial gripping attitude generation relies on the module's built-in solidified algorithm to run autonomously. It improves the coarse positioning accuracy by relying on geometric feature layering and cylindrical model iterative fitting, thereby reducing the attitude correction amplitude after near-field measurement from the source.

[0011] After the binocular vision module completes the hierarchical feature segmentation of the target object point cloud cluster and the connecting part point cloud cluster, it immediately initiates data preprocessing of the connecting part point cloud cluster and solves for the cylinder axis parameters. This relies on improved spatial voxelization downsampling to achieve lightweight processing of point cloud data. Then, through a parameter-optimized random sampling consensus algorithm, iteratively completes the optimal fitting of the preset cylinder model. Based on filtering out random noise and branch and leaf noise interference generated by sensor acquisition, the axis parameters of the fruit stalk connecting part are solved. Finally, the principal direction vector is normalized to generate a coarse axis vector. The obtained coarse axis vector is directly sent to the back-end pose solution stage to participate in the conversion of the initial adjustment amount of the end effector, providing the core geometric basis for determining the initial clamping posture. The spatial voxel is based on the three-dimensional connection part point cloud. The cubic spatial grid units, divided by the distribution range, are used for spatial voxel downsampling, a preprocessing method that compresses the disordered original point cloud according to the spatial grid assignment rules. This invention adds sparse voxel differentiation preservation processing logic to the traditional equidistant voxel sampling. When the downsampling is carried out, the coordinates of all connected part point clouds are traversed first, and the maximum and minimum coordinate values ​​corresponding to the three coordinate axes X, Y, and Z are extracted. The six extreme coordinates form a three-dimensional spatial bounding box that encloses the entire point cloud. Then, the side length parameters of the individual voxel cubes, which are pre-stored after the orchard is located, are retrieved. These parameters are divided into multiple sets of selectable fixed values ​​according to the thickness of the fruit stalk of different fruit trees. After selecting the corresponding side length according to the currently identified crop type, the three bounding boxes are used respectively. The axial span length is divided by the side length of a single voxel, and the result of the division is rounded up to obtain the number of voxel grids in the X, Y, and Z directions. Based on the number of grids, the entire bounding box is uniformly divided into a defined number of non-overlapping cubic voxel grids. After the grid is divided, each 3D coordinate point inside the original connection part point cloud is traversed one by one. The voxel grid number of a single point is determined by the conversion relationship between coordinates and voxel side lengths. Then, a differential retention judgment is initiated. There is a preset sparse point threshold. The total number of original point clouds falling inside a single grid is counted. When the number of point clouds inside the grid is less than the sparse point threshold, it is determined that the voxel corresponds to a sparse region of the fruit stalk edge. All original coordinate points inside the grid are directly retained to preserve the fruit stalk. For fine contour features, when the number of points in the mesh is greater than or equal to the sparse point threshold, the voxel is determined to correspond to the dense region of the fruit stalk. The arithmetic mean of the X, Y, and Z coordinates of all points in the mesh is calculated. A set of mean coordinates is used to replace all the original points in the mesh. After traversing all voxel meshes, all the remaining coordinate points are summarized to form a downsampled point cloud cluster. The point cloud cluster compresses redundant data to reduce the computational load of subsequent iterations and avoids excessive deletion of fine subdivisions of the fruit stalk features. The random sampling consensus method is a model parameter iterative fitting algorithm with strong noise resistance. It relies on multiple random samplings to build candidate models and then combines geometric error to screen effective interior points and remove outliers. This invention optimizes and improves the original iterative logic.The constraint of a fixed number of iterations is removed, and a dual dynamic termination rule is set. The first rule terminates the iteration directly when the maximum internal point statistical value no longer increases after ten consecutive iterations. The second rule forces the calculation to end after the number of iterations reaches a preset safety upper limit. The loop exits when either rule is met. The preset cylindrical model is a standardized spatial geometric model that fits the shape of the fruit stem cylinder. It consists of three parameters: the spatial axis fixed point, the three-dimensional direction vector of the axis, and the fixed cylinder radius. When entering a single iteration step, five spatial points are randomly selected without replacement from the point cloud cluster to form a dedicated point cloud subset for this round. The number of five points selected is determined through multiple batches of fruit stem geometric calibration to meet the solution conditions of the spatial cylindrical surface equation. Based on the three-dimensional coordinates of the five points within the subset... The direction of the cylinder's axis is calculated by simultaneously applying the constraint equations of the spatial cylinder. The calculation process first determines the three-dimensional coordinates of the points the axis passes through using spatial location data. Then, based on multi-point spatial geometric constraints, the component values ​​of the axis in the X, Y, and Z axes are calculated. These three components combine to form the original axis direction vector obtained in this fitting round. Distance consistency is a quantitative standard used to judge whether a point falls on the surface of the cylinder. It refers to the fact that the vertical distance from spatial points belonging to the same outer wall of the cylinder to the fitted cylinder axis will be concentrated within a narrow, reasonable range. Points deviating from this range are considered abnormal external points deviating from the cylinder's shape. The critical distance threshold is pre-calibrated based on field stalk measurement data. The selection of internal points is completed based on the vertical distance calculation results. The vertical distance is calculated as follows: A spatial vector is formed by selecting the point to be measured and the fixed point of the axis. A cross product operation is performed on this spatial vector and the axis direction vector, and the magnitude of the cross product is calculated. The vertical distance from a single point to the axis is then obtained by dividing the magnitude of the cross product by the magnitude of the axis direction vector itself. After calculating the distance for all points in the point cloud cluster, points with a vertical distance less than a critical distance threshold are designated as interior points in this iteration, while points with a vertical distance greater than or equal to the critical distance threshold are designated as invalid exterior points. The total number of interior points in this iteration is counted, and the axis parameters and the number of interior points in this iteration are stored simultaneously. This process of randomly sampling subsets and solving for the cylindrical axis is continuously repeated. The process involves iteratively calculating vertical distances point by point and dividing the points into inner and outer points. After each iteration, the number of inner points in the current iteration is compared with the globally stored maximum number of inner points. If the current value is larger, the global maximum number of inner points and the corresponding axis parameters are updated. After all iterations meet the termination condition, the original axis direction vector corresponding to the set of cylinder models with the largest number of inner points in the global storage is extracted. Then, a normalization operation is performed. The normalization method is to square the three coordinate components of the original axis direction vector, add the three squares, and then take the square root to obtain the vector magnitude. Finally, the X and Y components of the original vector are used. The Z-component is divided sequentially by the obtained modulus to obtain a unit direction vector with a modulus always equal to one. This unit direction vector is the principal direction vector of the point cloud cluster of the connecting part. After the principal direction vector is generated, it is directly output as the coarse estimation axis vector of the connecting part. The coarse estimation axis vector is then sent to the pose calculation submodule of the binocular vision module to perform vertical constraint alignment calculation with the pre-stored preset standard working plane normal vector. Based on the inverse kinematics model of the manipulator, the initial adjustment amount corresponding to the pitch joint and yaw joint is inferred. The spatial attitude of the end effector corresponding to the initial adjustment amount is defined as the initial clamping attitude. The data of the initial clamping attitude will be processed by... The data bus is completely sent to the subsequent attitude arbitration decision unit for attitude deviation calculation with the spatial axis vector output by the near-field structured light unit. This supports the confidence probability weighted decision and angle adjustment command generation in step S3. For example, after differential voxel downsampling, 70% of redundant dense points are removed from the point cloud corresponding to the apple stem. The maximum number of inner points in the whole cycle is obtained in the 37th round of dynamic iteration. After the corresponding axis is normalized, the coarse axis vector is obtained and used to solve the subsequent joint angle. The whole set of calculations runs autonomously by relying on the hierarchical optimization algorithm, and fully realizes the full-link automated conversion from the original point cloud to the coarse axis parameters.

[0012] After the end effector reaches the near-field range defined by the activation threshold and completes hardware triggering, the near-field structured light measurement unit continuously projects structured light spots with a uniquely coded arrangement rule at the connection point of the target object, namely the cylindrical fruit stalk area where the fruit connects to the branch. A high-speed macro camera continuously captures reflected light images frame by frame as the robotic arm moves towards the target object at a constant speed. All images are arranged and stitched together in the order of hardware acquisition to form a complete light band image sequence. The entire process, from image acquisition, centerline extraction, cross-frame deformation tracking to 3D contour reconstruction and axis fitting, uses a multi-stage improved algorithm to achieve high-precision solution of the local geometric parameters of the fruit stalk. The output spatial axis vector of the connection point can be corrected. The positioning deviation caused by the coarse estimation of the axis vector in the far-field binocular vision output provides a geometric benchmark for the subsequent attitude arbitration decision unit to carry out dual-source attitude data fusion and deviation calculation. The structured light spot is a combination of directional projection beams equipped with custom linear codes. The projection beams are arranged sequentially according to fixed intervals and coding numbers. Each beam with a different number corresponds to a unique coding identifier. After the beams contact the non-Lambertian surface of the connection part, diffuse reflection occurs, forming a strip-shaped reflective area, i.e., the imaging light band, on the camera imaging plane. The coding identifier can distinguish the imaging areas corresponding to overlapping beams, avoiding the pixel classification confusion caused by multiple light bands. The light band analysis module first starts the extraction of the light band centerline of a single frame image. This step adds coding partition preprocessing and breakpoints to the traditional image skeleton thinning algorithm. The adaptive completion processing logic, for any frame of the original light band image within the sequence, first performs a dual-channel parallel preprocessing operation. The first processing channel completes the grayscale conversion of the original color image, and then uses an adaptive local threshold segmentation algorithm to distinguish the effective pixels of the reflected light band from the invalid pixels of the background branches, obtaining a binarized light band mask image. The second processing channel reads the embedded spot code pixel information in the image, and divides and labels the scattered light band blocks inside the mask image according to the code value. Consecutive reflected pixels with the same code are defined as the same independent light band. After partitioning, skeleton refinement operation is performed within each independent light band block to extract candidate pixels at the center position of the light band. For centerline breaks caused by illumination defects, the neighborhood eight-pixel traversal completion logic is activated. The algorithm searches for the encoding and grayscale attributes of eight adjacent pixels around the breakpoint. If adjacent pixels belong to the same coded light band block, a center pixel is added to the missing position of the breakpoint. All valid center pixels are then concatenated to form a continuous, unbroken single light band centerline. This process is repeated to traverse all coded light bands within a single frame to extract the centerlines of all light bands in the entire frame. After the centerlines of all frames are extracted, the algorithm proceeds to the tracking stage of light band displacement and deformation between consecutive frames. This step employs a cross-frame tracking scheme that combines coded feature descriptor matching with local curvature interpolation deformation solving. Each light band centerline of the current frame is divided into equidistant segments. Local pixel blocks are extracted from each segment, and the light spot encoding number and local grayscale mean value contained in the block are extracted as the segment-specific feature descriptor.Based on the similarity matching criterion of feature descriptors, the optimal matching segment is retrieved from all centerline segments in the next frame with the closest temporal sequence. This completes the one-to-one binding of pixel points of the same physical light band between consecutive image frames. After point binding, the pixel displacement value is obtained by subtracting the corresponding point coordinates of the previous frame from the matching point coordinates of the subsequent frame, thus representing the overall translational change of the light band. Then, multiple spatial sampling nodes are uniformly extracted for each centerline, and the local curvature between adjacent sampling nodes is calculated. The local deformation amplitude of the entire light band is statistically analyzed by the curvature difference of sampling nodes at the same position in consecutive frames. The displacement data and curvature change data of all sampling nodes are summarized to form a complete spatiotemporal deformation feature of the light band. This type of deformation data intuitively reflects the surface curvature of the connecting part of the fruit stalk and the camera approximation. The target-induced field-of-view scaling changes, after point tracking and deformation parameter statistics are completed for the entire image sequence, are addressed by the light strip analysis module, which uses a modified triangulation principle to perform reverse reconstruction of the local three-dimensional contour. The triangulation principle relies on the spatial emission position of the projected light rays and the camera's imaging pixel positions to construct spatial triangular constraints. This leverages triangular geometric relationships to achieve the fundamental theory of stereo vision, transforming two-dimensional pixel coordinates into three-dimensional spatial coordinates. This invention adds a spot coding constraint optimization term to the traditional triangulation. First, it retrieves the intrinsic parameters of the spot projector, the intrinsic parameters of the high-speed macro camera, and the fixed spatial relative pose extrinsic parameters between the two types of devices, permanently stored after factory calibration. Then, it combines these with the coding number inherent in the light strip to lock the spatial emission ray of the corresponding projected beam. The analytical expression is then used. Substituting the image coordinates of the center pixel of the light band into the camera pinhole imaging model, the analytical expression of the imaging ray is obtained. The intersection point of the emitted ray and the imaging ray corresponding to the same code in three-dimensional space is the three-dimensional coordinate of the fruit stalk surface corresponding to the current pixel. After solving the three-dimensional coordinates of all center pixels of a single light band point by point, all spatial points are concatenated according to the extension order of the light band on the image to form a single segment of local three-dimensional contour line. The contour lines of multiple light bands in the whole frame and the three-dimensional points supplemented by multiple temporal images are summarized and stitched together to generate a complete set of local three-dimensional contour lines within the illumination range of the light spot. After the contour line set is constructed, the least squares spatial straight line fitting algorithm is used to perform optimal axis fitting on all discrete three-dimensional coordinate points on the contour line. After optimal fitting, the optimal axis fitting is obtained. The direction vector corresponding to the obtained spatial straight line is the spatial axis vector of the connecting part. During the synchronous stage of solving the spatial axis vector, the reprojection error of the reconstructed points in each frame and the fitting residual of the straight line fitting process are substituted into the preset fuzzy evaluation function to generate a confidence probability characterizing the reliability of the axis. The spatial axis vector and the corresponding confidence probability form a complete set of data and are synchronously transmitted to the attitude arbitration decision unit. The attitude arbitration decision unit receives the initial adjustment amount corresponding to the initial clamping posture output by the far-field binocular vision and the high-precision spatial axis vector output in this step. It first transforms the coordinate system of the spatial axis vector into the base coordinate system of the manipulator and calculates the joint angle correction amount required to satisfy the mutual perpendicular constraint between the normal vector of the end effector working plane and the axis.The difference between the initial adjustment amount and the joint angle correction amount is used to obtain the attitude deviation. Subsequently, relying on confidence probability as a weighting coefficient, the two types of adjustment parameters are fused and decided within a preset decision time limit, generating adjustment angle commands for the pitch and yaw joints. These commands are ultimately sent to the adjustment execution control unit, where they are converted into drive signals by a predictive control algorithm to complete joint attitude adjustment. For example, in a citrus harvesting scenario, a high-speed macro camera continuously acquires twelve frames of images to form an image sequence. Frame by frame, the breakpoints are filled in using encoding to extract four light band center lines. After cross-frame feature matching, the average pixel shift of the light band is statistically obtained as 3.7 pixels, with local curvature deformation concentrated in the 3% to 5% range. Improved triangulation combined with encoding constraints reconstructs the local three-dimensional contour of the fruit stalk within a 3mm width range. The spatial axis vector of the fruit stalk is obtained by fitting a spatial straight line. This vector is used to correct the angle deviation caused by far-field coarse positioning. The entire chain operation from imaging acquisition to axis output relies on the module's built-in fixed algorithm for automatic operation, and a layered algorithm is used to achieve automated conversion from two-dimensional image information to high-precision three-dimensional axis parameters.

[0013] After the light band analysis module completes the full-dimensional reconstruction of the local three-dimensional contour line of the connecting part based on the light band deformation trajectory of multiple frames of images, it immediately performs reconstruction error calculation, straight line fitting residual statistics, and confidence probability conversion and generation in parallel. This quantitatively characterizes the data reliability of the single-wheel structured light axis solution. The generated confidence probability will serve as the core weight basis for the weighted fusion of dual-source attitude parameters by the attitude arbitration decision unit. It can dynamically adjust the correction priority of the axis according to the on-site imaging quality, effectively avoiding the attitude misadjustment problem caused by imaging distortion due to strong light scattering or unevenness of the fruit stem surface. The reprojection error is obtained by back-projecting the reconstructed three-dimensional contour coordinates onto the imaging plane of the high-speed macro camera to obtain the theoretical pixel coordinates, and comparing them with the actual light band center image retained in the original image. The pixel deviation value obtained after conversion from primitive coordinates is used in the calculation of single-frame reprojection error with a weighted calculation logic based on coded blocks. Before the formal calculation, the module retrieves the light band grouping information of the current frame after coded partitioning and labeling. Different coded numbers correspond to different beam projection positions. Changes in the curvature of the fruit stalk surface will cause differential interference to the imaging accuracy of different coded light bands. Based on multiple batches of field calibration data, independent weighting coefficients are preset for each type of coded partition. At the same time, the camera intrinsic parameters of the high-speed macro camera, which are embedded in the storage chip, are retrieved to obtain the camera pose extrinsic parameters corresponding to the current frame. The camera intrinsic parameters are a fixed set of parameters obtained from the factory calibration of the device, including imaging focal length, principal point pixel coordinates, and lens distortion correction parameters. The camera pose extrinsic parameters are used to characterize the high-speed macro camera's phase... For the translation and rotation parameters of the robot's base coordinate system, for a spatial point falling on a local 3D contour line, the 3D spatial coordinates are first converted into theoretical abscissa and ordinate on the imaging plane using the pinhole camera's inverse projection calculation rules combined with intrinsic and extrinsic parameters. Then, the original actual abscissa and ordinate of the point saved in the previous light band centerline extraction step are retrieved. The reprojection error of a single pixel is calculated by squared the difference between the theoretical abscissa and the actual abscissa, then adding the squared difference between the theoretical ordinate and the actual ordinate, and finally taking the square root of the sum of the two squared values ​​to obtain the original pixel error of the single point. The original pixel error of the single point is then multiplied by the weighting coefficient corresponding to the coding partition to which the point belongs to obtain the weighted projection of the single point. The error is calculated by iterating through all valid contour pixels in the current frame that participate in contour reconstruction, accumulating all single-point weighted projection errors, and dividing the total accumulated error by the total number of valid pixels in the current frame to obtain the single-frame weighted average reprojection error. This process is repeated for all image frames participating in 3D reconstruction according to consistent calculation rules. After summing the single-frame weighted average reprojection errors for all frames, an accumulation operation is performed. The accumulated result is then divided by the total number of image frames involved in the statistics to obtain the average reprojection error for the entire image sequence. A larger average reprojection error indicates a higher degree of distortion in the structured light imaging of this batch, and a lower reliability of the 3D contour reconstruction data. After calculating the average reprojection error, the process proceeds to the step of statistically analyzing the fitting residuals for spatial line fitting.The fitting residual is the quantified result of the spatial perpendicular distance from each discrete 3D point on the local 3D contour line to the optimal fitting spatial axis. This step uses a segmented weighted statistical approach based on distance intervals to calculate the overall fitting residual. After obtaining the spatial axis vector of the connecting part by fitting the spatial straight line using the least squares algorithm, all discrete 3D coordinate points on the contour line are extracted one by one. The calculation method for the perpendicular distance from a single point to the fitting axis is as follows: First, a spatial vector pointing from the three-dimensional point to be measured to the axis is constructed. The constructed spatial vector is then cross-multiplied with the axis direction vector. The magnitude of the cross-product is calculated and then divided by the magnitude of the axis direction vector itself to obtain the vertical distance of a single point. Three distance intervals are pre-defined using stem samples, and a unique residual weighting coefficient is assigned to each interval. The three intervals are: effective point intervals near the axis, transition point intervals, and noise / anomaly point intervals far from the axis. The vertical distance of a single point is multiplied by the corresponding interval's weighting coefficient to obtain the single-point weighted fitting residual. The total residual is obtained by summing the single-point weighted fitting residuals of all points. The total residual is then divided by the total number of effective contour points to obtain the final overall fitting residual used for fuzzy calculation. A higher numerical value indicates a greater degree of dispersion of the three-dimensional contour points, and a lower level of confidence in the spatial axis vector generated by the fitting. After both error indices are calculated, the confidence probability conversion stage begins. The fuzzy evaluation function is a multi-input single-output nonlinear mapping operation rule formed through extensive light variation and fruit stem material testing. Its function is to eliminate the dimensional differences between the average reprojection error and the overall fitting residual, and to uniformly map the two types of error values ​​to a fixed numerical range of zero to one to obtain the confidence probability. The fuzzy evaluation function uses a mapping logic of dual membership degree preprocessing combined with piecewise correction coefficients. Before the formal calculation, the module retrieves the pre-stored upper and lower critical thresholds of the average reprojection error and the overall fitting residual, and first normalizes the membership degree of the average reprojection error. The first membership degree is calculated by subtracting the current average reprojection error from the maximum critical threshold corresponding to the average reprojection error, and then dividing by the maximum critical threshold minus the minimum critical threshold. Using the same calculation logic, the second membership degree corresponding to the overall fitting residual is calculated based on the residual critical threshold. Both membership degree values ​​fall within the initial range of zero to one. A fixed ratio of first and second influence weights is preset, and the sum of the two weights is always equal to one. The initial fusion value is obtained by multiplying the first membership degree by the first influence weight and adding the second membership degree by the second influence weight. Then, based on the magnitude of the initial fusion value, three numerical intervals are divided, each configured with an independent scaling correction coefficient. If the initial fusion value is greater than 0.8, the first... The correction coefficients are used as follows: a second correction coefficient is selected if the initial fused value is between 0.4 and 0.8; a third correction coefficient is selected if the initial fused value is less than 0.4. After multiplying the initial fused value and the corresponding correction coefficient, the result is constrained to the range of 0 to 1 by boundary constraint logic. The final output constrained scalar value is the confidence probability. A confidence probability value close to 1 indicates higher reliability of the solution result of the spatial axis vector in this round, while a value close to 1 indicates lower reliability of the axis data. The calculated confidence probability is bundled and packaged with the spatial axis vector of the connecting part and transmitted to the attitude arbitration decision unit. The attitude arbitration decision unit receives the initial adjustment amount corresponding to the initial clamping attitude output by the far-field binocular vision and the calculated spatial axis vector.First, the spatial axis vectors are uniformly converted to the robot's base coordinate system. Based on the constraint that the normal vector of the end effector's working plane is perpendicular to the axis, the joint angle correction is calculated. The difference between the initial adjustment and the joint angle correction is used to obtain the attitude deviation. Within a preset decision time limit, the confidence probability is used as a weighting coefficient to fuse and decide the two types of adjustment parameters. If the confidence probability exceeds a preset reliability threshold, the joint angle correction is used directly to generate the adjustment angle command. If it does not exceed the threshold, a weighted average is performed according to the confidence probability weights to obtain the final adjustment parameters. The generated adjustment angle command continues to be transmitted down to the adjustment execution control unit. The adjustment execution control unit reads the real-time angle difference between the pitch and yaw joints, and generates a segmented joint angle control sequence based on the robot's dynamics model and predictive control algorithm. This sequence is converted into drive signals to control the servo driver to complete the joint attitude adjustment, ensuring that the end effector's working plane and the spatial axis of the connecting part meet the preset vertical constraint conditions. The objective quantification of axis reliability is achieved through hierarchical weighting and segmented fuzzy mapping calculation methods, providing a weighted reference benchmark for full closed-loop attitude control.

[0014] After the light strip resolution module completes local 3D contour reconstruction, reprojection error statistics, and fuzzy mapping conversion to generate confidence probabilities, it sends the spatial axis vectors based on the camera coordinate system and the corresponding confidence probabilities to the attitude arbitration decision unit. The attitude arbitration decision unit simultaneously retrieves the initial clamping attitude parameters stored in the previous far-field visual coarse localization stage, namely the initial adjustment amounts of the pitch joint and yaw joint. It then sequentially performs cross-coordinate system vector transformation, iterative solution of joint correction parameters, calculation of dimensional attitude deviations, and dynamic weighted adjudication based on confidence probabilities. This hierarchical and step-by-step operation logic separately manages the weight ratio of the far-field coarse localization data source and the near-field measurement data source, which can fully utilize the high-precision axes when the structured light imaging is excellent. The advantages of line correction can also preserve the far-field coarse positioning reference constraint when the reliability of the axis solution is low due to sudden changes in illumination or reflection from the fruit stem, avoiding the misalignment of the end gripper posture caused by single-source data anomalies. The final adjustment angle command obtained by conversion will flow directly downward as the input reference parameter of the adjustment execution control unit. The robot's base coordinate system takes the mechanical installation reference center point of the robot's base as the origin of the spatial coordinate system. The X-axis of the entire machine extends along the horizontal installation reference of the base, the Y-axis extends along the depth direction of the base, and the Z-axis extends along the vertical upward direction to establish a unified reference coordinate system for the entire machine. The original spatial axis vector obtained by near-field structured light based on data collected by a high-speed macro camera belongs to the camera's independent coordinate system. This invention abandons the traditional transformation of homogeneous transformation with fixed parameters. The method employs a coordinate transformation scheme with real-time wrist pose dynamic compensation. Homogeneous coordinate transformation relies on a rotation matrix and a translation vector to complete spatial coordinate mapping. The attitude arbitration decision unit first reads the instantaneous pitch and yaw angle values ​​transmitted in real time from the encoder of the robotic wrist. It then retrieves the inherent installation rotation parameters and fixed installation translation parameters of the macro camera relative to the robotic wrist, which are fixed after multi-position calibration at the factory. Based on the inherent parameters, a basic homogeneous transformation matrix is ​​generated. Subsequently, the rotation components within the basic transformation matrix are corrected item by item using the real-time acquired wrist joint deflection values ​​to offset the slight offset of the camera's relative pose caused by mechanical backlash during the robotic arm's movement. The corrected homogeneous transformation matrix is ​​then multiplied by the original spatial axis vector. The calculation sequentially transforms the coordinates of the X, Y, and Z components of the vector, ultimately obtaining the transformed spatial axis vector mapped to the robot's base coordinate system. After completing the vector coordinate system, the fine-tuning of the joint angle correction is performed. The end effector's working plane is a standard gripping plane defined during the robot's calibration phase, with the gripper's center as the reference. The normal vector of the working plane is a fixed three-dimensional spatial vector perpendicular to this plane. The preset gripping geometric constraint is that the normal vector of the working plane and the transformed spatial axis vector satisfy the spatial perpendicularity condition. The mathematical criterion for the perpendicularity of the two vectors is that the vector dot product is equal to zero. This step uses a cost function constraint plus step-by-step inverse kinematics calculation method, instead of directly applying existing robot inverse kinematics formulas for single-point solutions.The first step is to construct the end-effector pose optimization cost function. The main cost term is the square of the dot product of the normal vector and the axis vector. An additional term for physical travel constraints of the manipulator joints is added to ensure that the optimized end-effector pose falls within the effective motion space of the manipulator. A small-step gradient descent iterative algorithm is used to continuously fine-tune the six-DOF spatial pose of the end-effector. The real-time value of the cost function is recalculated in each iteration until the change in the cost function after five consecutive iterations is less than the preset minimum convergence threshold. At this point, the locked end-effector spatial pose is the optimal target pose that satisfies the vertical constraint. The second step is to decompose and convert the optimal target end-effector pose into target angles corresponding to the pitch joint and yaw joint, based on the pre-stored forward and inverse kinematic calibration parameters of the manipulator. The two sets of angle values ​​are integrated and defined as joint angle corrections. The pitch correction and yaw correction are stored separately for subsequent dimensional weighted calculations. After the corrections are solved, the attitude deviation is calculated. The pitch dimension attitude deviation is calculated as follows: The initial pitch adjustment is subtracted from the pitch joint angle correction. The yaw attitude deviation is calculated by subtracting the yaw joint angle correction from the initial yaw adjustment. These two sets of deviation values ​​visually represent the angular deviation between the far-field coarse-tuning parameters and the near-field fine-tuning parameters. All deviation data is retained for subsequent anomaly tracing. Afterward, the process enters the dimensional weighted fusion and adjudication stage, using confidence probabilities as weights. The attitude arbitration decision unit pre-programs a reliability threshold calibrated through field harvesting and a preset decision time limit for each adjudication. The reliability threshold is the critical confidence value that distinguishes usable and unusable axis data. The decision time limit constrains the maximum computation time of a single round of adjudication to avoid control lag. The unit's built-in decision cycle counter receives all input parameters... The timing begins the moment the calculation is completed and the count is accumulated cycle by cycle. This step implements the calculation logic of independent weighting of pitch and yaw parameters, no longer using the same set of weights to simultaneously calculate the two joint angles. The calculation is divided into two execution scenarios. The first scenario is when the confidence probability of real-time reading is greater than the preset reliability threshold. In this scenario, the reliability of the spatial axis data calculated by near-field structured light is determined to be up to standard. The reference influence brought by the preliminary adjustment in the far field is directly ignored, and the joint angle correction is directly assigned to the final adjustment angle of pitch and the final adjustment angle of yaw, respectively. The second scenario is when the confidence probability of real-time reading is less than the preset reliability threshold. In this scenario, the axis is determined to have obvious calculation errors, and the reference parameters of far-field coarse positioning need to be fused. The calculation method of the final adjustment angle of a single joint is as follows: The initial adjustment amount is multiplied by one, minus the confidence probability, and then added to the joint angle correction amount multiplied by the confidence probability. This calculation formula is then substituted into the pitch and yaw data sets to perform independent weighted average calculations for the two sets of parameters. The entire weighted fusion calculation must be completed within the preset decision time limit before the cumulative duration of the decision counter exceeds the limit. If the confidence probability rises and exceeds the reliability threshold before the counter reaches the upper limit, the weighted calculation is immediately terminated and the value selection rules for the first scenario are switched. If the counter count reaches the preset decision time limit but the confidence probability remains below the reliability threshold, the result of the current period's weighted average calculation is directly used as the final adjustment value. The pitch adjustment value generated after scenario determination and the yaw adjustment value are then compared. The pendulum adjustment values ​​are uniformly encapsulated to form a complete adjustment angle command. The adjustment angle command is transmitted to the adjustment execution control unit through the internal data bus. After receiving the command, the adjustment execution control unit reads the current actual angles of the pitch and yaw joints. Based on the manipulator's dynamic model and predictive control algorithm, it predicts the motion overshoot and back-optimizes to generate a joint angle control sequence. At the same time, an adaptive Kalman filter is introduced to smooth the real-time expected angle. Finally, it is converted into a servo drive signal to drive the joint operation, so that the working plane of the end effector and the converted spatial axis always maintain the preset vertical geometric relationship. Intelligent optimization fusion of dual-source attitude data is achieved by relying on dimensional weighting and dynamic time limit control.

[0015] After the attitude arbitration decision unit completes the calculation of attitude deviations in both pitch and yaw dimensions and configures the weighted fusion basic parameters, it immediately initiates parameter initialization and periodic cyclic adjudication using the built-in decision cycle counter. Relying on segmented counting control, it achieves rigid constraints on the adjudication time limit. This not only reserves a reading window for multiple confidence probability updates, waiting for the near-field structured light module to update the axis data with higher reliability, but also prevents the indefinite delay of control commands caused by long-term low confidence probabilities due to causal armature reflection and ambient stray light interference. This ensures that the generated adjustment angle commands are delivered to the adjustment execution control unit on time to complete subsequent joint drive calculations. The decision cycle counter is a programmable software counting register integrated within the attitude arbitration decision unit's main control chip. It is used to equally divide the preset overall decision time limit and control the adjudication duration through discrete periodic counting. Before the device is officially put into use, full parameter calibration and configuration definition are completed. The first step is to set the parameters for the total decision time limit and the duration of a single decision cycle. The total decision time limit is a fixed time parameter obtained by combining the orchard robot's travel speed and the structured light image refresh rate through multiple batches of field harvesting calibration. The unit is uniformly set in milliseconds. The duration of a single decision cycle is the minimum hardware computation time required to complete the confidence probability reading, parameter weighting calculation, and threshold determination in a single operation. This duration is calculated from the inherent clock cycle of the unit's main control crystal oscillator. An additional cycle adaptation delay configuration is added. The basic duration of the single decision cycle is fine-tuned in combination with the image output refresh cycle of the near-field high-speed macro camera, so that each decision cycle exactly matches the output interval of a new light band data frame, avoiding the waste of computing power caused by repeatedly reading outdated confidence probabilities that have not yet been updated. The second step is to complete the numerical calculation of the upper limit of the number of decision cycles. The calculation method for the upper limit of the number of cycles is as follows: The total decision time is divided by the duration of a single decision cycle, and the result is rounded up. The positive integer after rounding is the maximum count value that the counter can accumulate, which is the upper limit of the number of cycles. At the same time, the storage width of the counter register is selected according to the number of bits of the upper limit value, and storage space is reserved for hardware overflow protection. The third step is to configure the counter start and stop rules and the initial assignment logic. The counter has a preset clear register. When the buffer of the attitude arbitration decision unit fully receives the initial pitch and yaw adjustment corresponding to the initial clamping attitude, the spatial axis vector after coordinate system transformation, and the first round of confidence probability, the clear register triggers a reset instruction, the real-time count value of the counter is directly reset to zero, and the internal hardware timing of the chip is started simultaneously. The clock officially enters the periodic cumulative counting state. The reliable threshold is a fixed dimensionless critical value obtained from the previous calibration based on massive fruit stalk structured light measurement experiments. The value falls within the range of zero to one and is used to determine whether the spatial axis vector of the near-field solution meets the usage standard of the fully corrected far-field coarse adjustment parameter. After entering the cyclic operation of each decision cycle, the real-time count value is incremented by one after the hardware timing duration corresponding to each single decision cycle is completed. Within each complete decision cycle, the real-time data buffer of the optical band resolution module is accessed first through the hardware data interaction bus to retrieve the current confidence probability generated by the latest refresh after the fuzzy evaluation function. After the retrieval operation is completed, a dual-condition logic judgment is carried out simultaneously. The first judgment content is the current... If the latest confidence probability value is greater than the preset reliability threshold, and the second criterion is that the current real-time counter value is equal to the pre-calculated upper limit of the number of cycles, the subsequent cycle loop will be terminated immediately if either of the two criterion conditions is met, and the counting will exit and enter the final instruction generation stage. For the first scenario of early termination, that is, in any decision cycle before the real-time counter value has reached the upper limit of the number of cycles, if the latest confidence probability read has exceeded the reliability threshold, the pitch and yaw weighted result calculated according to the weighted average formula for this cycle will be discarded directly. Instead, the joint angle correction amount calculated based on the spatial axis vector stored in the cache for this cycle will be retrieved, and the pitch and yaw values ​​of the joint angle correction amount will be directly encapsulated into the final adjustment angle. The instruction processing logic prioritizes near-field measurement data with higher accuracy, fully leveraging the correction advantages of high-precision structured light measurement. For the second scenario where the operation terminates upon reaching the upper limit—that is, when the confidence probability of all rounds during the full-cycle counting process remains below the reliability threshold until the counter count reaches the cycle limit and triggers the termination condition—the weighted average value obtained through confidence probability weighting calculation for the current cycle is directly used as the final adjustment parameters for pitch and yaw. This is then encapsulated to generate the corresponding adjustment angle instruction. The entire cycle counting and decision-making logic also includes a hardware fault fallback mechanism. If a main control clock failure causes the counter count to overflow beyond the cycle limit, the register automatically latches the calculation result of the last valid decision cycle as the output instruction.The generated angle adjustment command is transmitted to the adjustment execution control unit via the unit output interface. Upon receiving the command, the adjustment execution control unit reads the current actual angles of the pitch and yaw joints, calculates the difference between the target angle and the real-time angle, and predicts overshoot and oscillation caused by joint movement based on the robot's inherent dynamics model and predictive control algorithms. It then reverse-optimizes and generates a segmented joint angle control sequence. Simultaneously, an adaptive Kalman filter algorithm is used to smooth the real-time desired angle. During the filtering process, the process noise covariance matrix of the algorithm can be dynamically modified based on the confidence probabilities stored in historical periods. Finally, the optimized control sequence is converted into servo drive electrical signals and sent to the servo drivers of the corresponding joints in a time-division manner, driving the pitch and yaw joints to complete the angle deflection. Ultimately, this achieves a preset perpendicular relationship between the end effector's working plane and the spatial axis vectors in the robot's base coordinate system. The entire closed loop, from counter initialization to command issuance, relies on the attitude arbitration decision unit's built-in program for autonomous operation, balancing the contradiction between data waiting time and system control real-time performance through a segmented periodic counting control mode.

[0016] After the attitude arbitration decision unit completes the periodic judgment calculation and generates the adjustment angle command carrying the target angles of the pitch and yaw joints, the command is transmitted to the adjustment execution control unit via the high-speed data interaction bus inside the device. The adjustment execution control unit then initiates predictive trajectory planning and servo drive control based on the dynamic model. This control mode relies on advance multi-cycle motion prediction to correct the control output in advance, avoiding overshoot and oscillation defects during joint operation from the trajectory design level. Unlike the traditional hysteresis closed-loop control that passively corrects motion deviations after they occur, this mode can be combined with adaptive Kalman filtering to smooth the desired angle, further ensuring... To ensure the smoothness of the end effector's attitude adjustment and ultimately reliably achieve the vertical assembly constraint between the end effector's working plane and the spatial axis vector of the connecting part, the predictive control algorithm is a feedforward closed-loop composite control algorithm that uses the inherent dynamic parameters of the controlled machine to perform multi-step future working condition extrapolation and combines the prediction results to optimize the control output in advance. The operating logic is to use the deviation data between the current actual angle and the target angle, substitute it into the fixed equipment dynamic equation, and extrapolate the motion changes in subsequent control cycles. The trajectory optimization is completed before the actual position error of the mechanical action occurs. The known dynamic model of the robot is fixed in the control system after no-load calibration and actual measurement of different fruit load groups. The rigid body dynamics equations within the unit's storage chip fully incorporate the inherent parameters of the pitch and yaw joints, including their moments of inertia, shaft viscous damping coefficients, servo transmission friction coefficients, and linkage cross-axis inertial coupling coefficients. The angle adjustment command is a digital message encapsulating the target control parameters. This message internally stores two key values: the target angle of the pitch joint and the target angle of the yaw joint. The current actual angle is the instantaneous shaft angle collected and transmitted in real-time by photoelectric encoders installed at the pitch and yaw joint shafts at a fixed sampling frequency. The angle difference is the deviation obtained by subtracting the corresponding real-time actual angle from the target angle of a single joint. A single control... The period is the preset control signal refresh interval of the servo driver, a fixed duration determined by the servo hardware reference crystal oscillator. Overshoot refers to the positional offset caused by the actual operating angle of the joint exceeding the set target angle. Oscillation refers to the unsteady motion phenomenon of the joint repeatedly swinging back and forth in small amplitudes around the target angle. The adjustment execution control unit first performs message parsing processing on the received adjustment angle command, extracts the target angle of the pitch joint and the target angle of the yaw joint from the message partition, and simultaneously opens the data reading channels of the two joint encoders to capture the current actual angles of pitch and yaw in real time. The difference is calculated independently in two dimensions. The calculation method for the angle difference in the pitch dimension is as follows: The pitch joint target angle is subtracted from the current actual pitch joint angle. The yaw angle difference is calculated by subtracting the yaw joint target angle from the current actual yaw joint angle. These two angle differences are stored in their respective dedicated data registers, serving as the core input data source for subsequent predictive control algorithms. Before the difference data is formally fed into the predictive algorithm, an adaptive Kalman filter is called to smooth and reduce noise in the original target angle. The noise covariance matrix of the filtering process is dynamically corrected based on the confidence probability stored in historical cycles, filtering out small jumps in the target angle caused by encoder noise. The smoothed target parameters synchronously participate in subsequent trajectory optimization calculations, and then enter the predictive control algorithm. Based on the known dynamic model of the robot arm, the algorithm predicts subsequent multi-dimensional changes based on the angle difference. In the calculation stage of controlling cycle overshoot and oscillation, this invention adds dynamic load disturbance real-time compensation processing logic to the traditional fixed model iterative prediction. At the beginning of the algorithm operation, the number of forward prediction steps, which is calibrated in advance according to the field harvesting conditions, is retrieved. The number of prediction steps represents the total number of subsequent control cycles that need to be continuously deduced in a single operation. The first step loads the already discretized manipulator dynamics model into the calculation cache. The step size of the discretization processing is kept completely consistent with the fixed duration of a single control cycle. The second step uses the instantaneous angular velocity and instantaneous angular acceleration of the joints collected at the current moment as the initial state variables of the dynamics model. The angle difference obtained from the multidimensional calculation is converted into the servo equivalent driving torque and incorporated into the model input. At the same time, the previous round of harvesting data is retrieved from the real-time cache. The equivalent load disturbance torque of the corresponding fruit is added to the torque balance term of the dynamic model to compensate for the real-time load offset problem caused by the inability of the standard dynamic model to adapt to fruits of different weights. The third step starts from the first control cycle to be predicted and substitutes the values ​​into the model in chronological order for recursive calculation. The theoretical joint angle, instantaneous angular velocity, and instantaneous angular acceleration corresponding to each prediction cycle are calculated in turn. After the calculation is completed, the values ​​of the theoretical angle and the target angle of a single cycle are compared one by one. If the value of the theoretical angle of a single cycle is greater than the target angle, it is marked that there is a positive overshoot in that cycle. If the angle values ​​of multiple consecutive adjacent prediction cycles fluctuate alternately on both sides of the target angle, it is determined that there is a continuous oscillation in the corresponding interval. The entire prediction cycle is statistically analyzed. The maximum overshoot amplitude and the number of cycles occupied by the oscillation are two quantified abnormal characteristic parameters stored separately as the basis for judging the subsequent reverse optimization control sequence. After completing the multi-cycle motion state prediction and abnormal parameter statistics, the process enters the process of generating joint angle control sequences based on the prediction results through reverse optimization. The joint angle control sequence is a set of time-series parameters composed of the expected joint rotation angles of each subsequent control cycle arranged in chronological order. The total length of the sequence is consistent with the preset forward prediction steps. The maximum allowable motion velocity threshold and the maximum allowable angular acceleration threshold of the pitch and yaw joints, respectively, are pre-entered through mechanical limit tests. The velocity and acceleration parameters corresponding to any point in the sequence generated throughout the optimization process cannot exceed the preset thresholds.The first step involves generating an initial reference control sequence using linear uniform interpolation. The reference sequence's point changes linearly from the current actual angle to the target angle at a uniform speed. The second step retrieves the maximum overshoot amplitude and oscillation period data obtained from previous calculations. For the latter half of the sequence points where overshoot is predicted, the expected angle value for the corresponding period is proportionally reduced according to the overshoot amplitude's proportion to the target travel. For intervals where continuous oscillation is predicted, the original linear interpolation points are discarded, and S-shaped acceleration / deceleration curves are used to refill all sequence points within the interval. This slows down the rate of angular velocity change at the beginning and end of the interval, suppressing the inducing reciprocating oscillation. The third step initiates multi-round convergence iteration verification. After each sequence point correction, the updated control sequence is reintroduced into the discrete dynamics model for multi-cycle motion prediction. This iteration continues until no overshoot or oscillation exceeding the limits appears in the new full-cycle prediction result. Once the iteration meets the convergence condition, the final optimized joint angle control sequence is locked. The optimized control sequence is complete from the design source. By establishing boundary constraints on velocity and acceleration, and avoiding mechanical motion anomalies caused by unreasonable trajectories, the execution control unit is adjusted to extract the desired single-cycle rotation angle sequentially from the optimized control sequence according to the control cycle sequence. Using the servo rotation angle drive conversion formula, the angle value is converted into a pulse drive signal or analog voltage drive signal matching the servo driver system. Following a time-division multiplexing rule, the drive signals are sent periodically to the pitch joint servo driver and yaw joint servo driver. After receiving the drive signals, the servo drivers drive the internal servo motors to rotate, causing the corresponding joints to complete the angle deflection. During operation, the encoder continuously transmits updated actual angles in real time, forming a closed-loop correction, gradually pushing the end effector towards the target gripping posture. Ultimately, the working plane of the end effector and the spatial axis vector of the connection part in the robot's base coordinate system maintain a preset perpendicular geometric relationship. This pre-predictive optimization architecture reduces the probability of mechanical vibration failures under complex orchard conditions, improving fruit gripping and positioning accuracy.

[0017] After the light strip analysis module completes the reconstruction of the local 3D contour lines of the multi-frame connection section, solves the spatial axis vector, and generates the confidence probability corresponding to the single-round solution result, it performs a pre-weighted moving average filtering process on the spatial axis vectors corresponding to the multiple local 3D contour lines output in batches according to the time sequence of image acquisition. This pre-filter is used to cancel the instantaneous irregular jitter of the axis vector caused by the deformation of the structured light projection spot and the random imaging noise of the high-speed macro camera. The smoothed and optimized spatial axis vector, along with the matched confidence probability, is transmitted to the attitude arbitration decision unit. The attitude arbitration decision unit uses the coordinate transformation logic of dynamic compensation to convert the spatial axis vector to the robot's base coordinate system, and then combines it with the normal vector of the standard working plane of the end effector. The vertical constraint is used to solve for the joint angle correction. The decision cycle counter controls the time limit rules and confidence probability threshold judgment logic to complete the weighted fusion decision of the initial adjustment amount and the joint correction amount. Finally, an adjustment angle command containing the target angles of the pitch and yaw joints is generated. This command is sent to the adjustment execution control unit via the internal data bus. The control unit parses the target angle parameters, reads the current actual angle fed back by the joint encoder, and calculates the angle difference. This difference is then used as input to a predictive control algorithm based on the manipulator's dynamics model. Through multi-cycle overshoot and oscillation prediction, a discrete-time joint angle control sequence is generated through reverse optimization. For the real-time joint desired angle obtained from the cycle-by-cycle decomposition of the control sequence, a solution using a light strip is employed. The axis filtering in the analysis module uses a weighted moving average filtering operation with all configuration parameters independently differentiated, but it belongs to the same algorithm architecture. Subsequently, it is further optimized by an adaptive Kalman filter that dynamically corrects the noise covariance matrix based on historical data. The hierarchical filtering with different parameters in the upstream and downstream sections with the same architecture can suppress numerical jumps step by step from the source measurement data end and the end control command end, respectively. This reduces attitude decision offsets caused by axis data jitter due to fruit stalk reflection and forest stray light interference, and prevents small reciprocating vibrations of the pitch and yaw joints caused by frequent sudden changes in drive commands. It effectively ensures that the spatial axis of the end effector clamping plane and connecting part maintains the preset vertical geometric constraint for a long time. The spatial axis vector is based on the contour reconstructed by structured light triangulation. The three-dimensional direction parameters obtained by linear fitting are used in a weighted moving average filter, which forms the basic filtering architecture shared by the upstream and downstream components of this invention. The only difference lies in the configuration of different window lengths, weighting coefficients, and anomaly detection thresholds in the optical band analysis module and the adjustment execution control unit. The real-time joint desired angle is extracted sequentially from the optimized joint angle control sequence according to a single servo control cycle, representing the pitch and yaw target angle values ​​for each cycle. When the adjustment execution control unit performs filtering operations of the same type but different parameters, it employs three processing logics: variable window length based on different working conditions, dynamic weighting linked to error, and pre-elimination of abrupt value changes. Based on the absolute value of the difference between the target angle and the actual angle, three types of mechanical adjustment working conditions are pre-defined: the first type is a large-angle attitude adjustment working condition, and the second type is a small-amplitude fine-tuning working condition.The third category is the steady-state maintenance condition. Through pre-calibration using multiple batches of orchard harvesting, specific filter window lengths are assigned to each of the three conditions. For large-angle attitude adjustment, a shorter window with a smaller value is used to retain effective information about angle changes. For minor fine-tuning, a medium-length window is used. For steady-state maintenance, the maximum-length window is used to maximize the filtering of random fluctuations. After the window parameters are selected, the actual joint motion error corresponding to the preceding control cycles stored in the control unit's loop buffer is retrieved. The actual joint motion error is calculated by subtracting the original real-time expected joint angle before filtering from the measured joint angle of the servo encoder in a single cycle. The reciprocal of the motion error for each single cycle is calculated, and then all reciprocals are normalized to obtain the weighted coefficient of the corresponding data for the current cycle. The larger the error value, the smaller the corresponding weighting coefficient. This weighting weakens the interference of abnormal periodic data with large errors on the filtering results. Before the formal weighted average calculation, a step to eliminate excessive abrupt changes is added. A preset angle abrupt change threshold is used. The difference between the expected joint angles of the original real-time joints in two adjacent control cycles is compared sequentially. When the absolute value of the difference exceeds the abrupt change threshold, the original data for that period is directly eliminated. The angle value from the previous cycle, after initial smoothing, is used to temporarily fill the gap. After the anomaly elimination is completed, valid samples are extracted from the remaining valid data range according to the filtering window corresponding to the current operating condition. The expected angles of each cycle within the sample are multiplied by the corresponding normalized weighting coefficient. All products are accumulated and then divided by the sum of the weighting coefficients. Finally, the real-time expected joint angle after basic smoothing is obtained for a single channel. The pitch and yaw channels are filtered independently. After the initial filtering of the two parameters, they are immediately sent to the adaptive Kalman filter module for secondary optimization. The adaptive Kalman filter is a closed-loop filtering algorithm that relies on state prediction and observation feedback to iteratively correct the state estimate. The process noise covariance matrix is ​​used to quantify the random fluctuations in state caused by internal modeling errors and environmental disturbances. Traditional fixed-parameter Kalman filtering uses a constant process noise covariance throughout the process. This invention achieves dynamic adjustment of matrix parameters by relying on two types of measured data. The two data sources are the multi-cycle historical confidence probabilities retained by the upstream optical band analysis module and the multi-cycle actual joint motion errors statistically analyzed by the local encoder. The execution control unit allocates a fixed-length circular buffer space in the storage area to continuously store historical confidence probabilities and channel motion errors for the most recent specified number of scheduling cycles. During the equipment's factory calibration phase, five confidence probability grading intervals and five motion error grading intervals are pre-defined. Each interval is matched with an independent reference scaling factor. The first step dynamically adjusts the historical confidence probabilities within the buffer and calculates their arithmetic mean. The confidence matching scaling factor is extracted by comparing the average value with the grading interval in which it falls. A higher overall confidence probability value indicates higher reliability of the near-field structured light axis solution, and the corresponding confidence matching scaling factor value is smaller; conversely, the scaling factor increases accordingly. The second step calculates the arithmetic mean error of all historical joint actual motion errors within the buffer.The first step involves extracting the error correction scaling factor based on the error grading interval where the average error falls. The second step uses the factory-calibrated baseline process noise covariance matrix as a basis, multiplying all elements of the matrix by the confidence matching scaling factor, and then by the error correction scaling factor. Simultaneously, based on the robot arm linkage coupling calibration parameters, the diagonal elements of the matrix corresponding to pitch and yaw are fine-tuned, while the off-diagonal coupling elements are synchronously and slightly corrected according to the linkage inertia coupling coefficient. After three layers of scaling and sub-item fine-tuning, a completely updated process noise covariance matrix is ​​obtained. The updated matrix is ​​directly substituted into the state prediction stage of the next cycle's adaptive Kalman filter. The Kalman filter internally performs the initial smoothing... The real-time expected angle of the joint is set as the system state variable, and the actual joint angle transmitted back by the encoder in real time is used as the observation variable. A complete set of standard calculations is executed sequentially, including one-step state prediction, observation equation update, Kalman gain iterative correction, and optimal state estimation output. The final expected angle after Kalman secondary optimization is then converted into a standard servo drive signal and sent sequentially to the pitch joint servo driver and yaw joint servo driver according to the time-division multiplexing rule. The drive motor drives the rotating shaft to complete the angle deflection. Relying on the mechanism of adaptive operating condition parameters and upstream and downstream measurement quality linkage correction, end-to-end noise suppression is achieved, adapting to the complex harvesting environment of orchards with variable lighting and random fruit load.

[0018] Please see Figure 2 As shown, a second objective of this invention is to provide a method for implementing an intelligent control system for the spatial gripping posture of an orchard harvesting robot, comprising the following steps: S1. When the distance between the end effector and the target exceeds the activation threshold, the scene point cloud containing the target and the connecting part is acquired through the binocular vision module. The scene point cloud is segmented to obtain the connecting part point cloud cluster. The main direction vector of the connecting part point cloud cluster is calculated as the coarse estimation axis vector. Based on the coarse estimation axis vector, the initial adjustment amount of the pitch joint and yaw joint that makes the preset standard working plane normal vector of the end effector perpendicular to it is calculated. The spatial attitude of the end effector corresponding to the initial adjustment amount is determined as the initial clamping attitude. S2. When the end effector enters the near-field region within the activation threshold of the target object, a measurement is triggered. A structured light spot with an coded pattern is projected onto the connection region of the target object by a light spot projector fixed to the mechanical wrist. A high-speed macro camera collects the image sequence of the non-Lambertian reflective light band formed by the structured light spot on the surface of the connection. The center line of the light band in each frame is extracted based on the initial adjustment amount, and the displacement and deformation of the center line of the light band between consecutive frames are tracked. Based on the continuous deformation trajectory of the center line of the light band, the local three-dimensional contour line of the connection surface in the light band illumination area is reconstructed in reverse through the triangulation principle. The local three-dimensional contour line is fitted with a spatial straight line to obtain the spatial axis vector of the connection. At the same time, a confidence probability characterizing the reliability of the spatial axis vector solution result is generated. S3. Receive the initial clamping posture and its corresponding preliminary adjustment amount, as well as the spatial axis vector and its corresponding confidence probability. Transform the spatial axis vector into the robot's base coordinate system and calculate the joint angle correction amount required to make the normal vector of the end effector's working plane perpendicular to the vector. Calculate the posture deviation between the preliminary adjustment amount and the joint angle correction amount. Using the confidence probability as the weight, within a preset decision time limit, perform weighted fusion and adjudication on the adjustment tendency based on the preliminary adjustment amount and the adjustment tendency based on the joint angle correction amount to generate the final adjustment angle commands for the pitch and yaw joints. S4. In the final stage before the end effector executes the target action, the current actual angles of the pitch and yaw joints are read according to the angle adjustment command, the difference between the angles and the target angles is calculated, and a predictive control algorithm is used based on the manipulator dynamics model to predict the motion state in the subsequent control cycle with the difference as input. The joint angle control sequence is then generated by reverse optimization, and the joint angle control sequence is converted into a drive signal to drive the pitch and yaw joints to move, so that the working plane of the end effector and the spatial axis vector satisfy the preset perpendicularity relationship, thus completing the closed-loop intelligent control of the spatial clamping posture.

[0019] 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 present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely preferred examples and are not intended to limit the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A spatial gripping posture intelligent control system for an orchard harvesting robot, wherein the robotic arm has at least a wrist, an end effector, a pitch joint, and a yaw joint, characterized in that, Also includes: The far-field vision coarse localization unit is used to acquire scene point clouds of the target object and its connecting parts through a binocular vision module when the distance between the end effector and the target object exceeds the activation threshold, and to calculate the initial clamping posture of the end effector based on the scene point clouds. A near-field structured light measurement unit is fixed to the wrist of the robotic arm and is triggered when the end effector enters a near-field region within the activation threshold of the target object, including: A light spot projector is used to project a structured light spot with an coded pattern onto the connection area of ​​the target object; A high-speed macro camera is used to acquire image sequences of non-Lambertian reflective light bands formed by the structured light spot on the surface of the connection part. The light strip analysis module is used to calculate the spatial axis vector of the connecting part based on the spatiotemporal deformation characteristics in the non-Lambertian reflective light strip image sequence, including: As the end effector approaches the target, the high-speed macro camera continuously acquires multiple frames of light strip images to form an image sequence. The light strip analysis module extracts the center line of the light strip formed by the structured light spot on the surface of the connector from each frame of the image sequence. It tracks the displacement and deformation of the center line of the light strip between consecutive image frames in the image sequence. The displacement and deformation reflect the continuous change of the curvature of the connector surface under the camera's view. Based on the continuous deformation trajectory of the center line of the light strip in the multiple frames of images, the light strip analysis module reconstructs the local three-dimensional contour line of the connector surface in the light strip illumination area through the principle of triangulation. Finally, it performs spatial straight line fitting on the local three-dimensional contour line, and the direction vector of the obtained straight line is the spatial axis vector of the connector. When reconstructing the local 3D contour line based on multiple frames of images, the light strip analysis module simultaneously calculates the reprojection error of the contour line point coordinate reconstruction for each frame of data and calculates the average reprojection error for all frames. At the same time, the light strip analysis module evaluates the fitting residual when performing spatial straight line fitting on the local 3D contour line. The confidence probability is generated by inputting the average reprojection error and the fitting residual into a preset fuzzy evaluation function. The fuzzy evaluation function maps the average reprojection error and the fitting residual into a scalar value between zero and one. The scalar value is the confidence probability, which is used to characterize the reliability of the spatial axis vector solution result. The attitude arbitration decision unit receives the initial clamping attitude and the spatial axis vector respectively, calculates the attitude deviation, and uses the confidence probability generated by the light strip analysis module in the solution as the weight to adjudicate and generate the adjustment angle command of the pitch joint and yaw joint within the preset decision time limit. In the final stage before the end effector performs the target action, the adjustment control unit drives the pitch joint and yaw joint according to the adjustment angle command, so that the working plane of the end effector and the spatial axis vector satisfy the preset perpendicularity relationship, thereby completing the closed-loop intelligent control of the spatial clamping posture.

2. The intelligent control system for spatial gripping posture of an orchard harvesting robot according to claim 1, characterized in that: The initial clamping posture of the end effector based on the scene point cloud computing specifically includes: The binocular vision module segments the scene point cloud to obtain the target object point cloud cluster and the connecting part point cloud cluster. It calculates the main direction vector of the connecting part point cloud cluster as the coarse estimation axis vector of the connecting part. It aligns the normal vector of the end effector in the space with the coarse estimation axis vector to obtain the initial adjustment amount of the pitch joint and yaw joint that makes the normal vector perpendicular to the coarse estimation axis. The spatial attitude of the end effector corresponding to the initial adjustment amount is defined as the initial clamping attitude.

3. The intelligent control system for the spatial gripping posture of an orchard harvesting robot according to claim 2, characterized in that, Calculating the principal direction vector of the point cloud clusters in the connectivity region specifically includes: Spatial voxelization downsampling is performed on the point cloud clusters of the connecting parts. A preset cylindrical model is iteratively fitted using a random sampling consistency method. In each iteration, a subset of the point cloud clusters of the connecting parts is randomly sampled to calculate the cylindrical axis direction of the preset cylindrical model. Inner points are screened by evaluating the consistency of the distances from all point clouds to the cylindrical axis direction. Finally, the axis direction vector of the cylindrical model with the most inner points is selected from all iteration results. The axis direction vector is normalized and used as the principal direction vector, and then output.

4. The intelligent control system for spatial gripping posture of an orchard harvesting robot according to claim 1, characterized in that: The attitude arbitration decision unit uses the confidence probability generated by the light strip analysis module during the calculation as weights, and within a preset decision time limit, it generates adjustment angle commands for the pitch and yaw joints, specifically including: The attitude arbitration decision unit receives the initial adjustment amount of the pitch and yaw joints corresponding to the initial clamping attitude and the spatial axis vector, and transforms the spatial axis vector into the manipulator base coordinate system. It calculates the joint angle correction amount that makes the normal vector of the end effector working plane perpendicular to the transformed spatial axis vector, and calculates the difference between the initial adjustment amount and the joint angle correction amount as the attitude deviation amount. When the attitude arbitration decision unit makes a decision, it uses the confidence probability as a weight to perform a weighted fusion of the adjustment tendency based on the initial clamping attitude and the adjustment tendency based on the spatial axis vector. If the confidence probability is higher than a preset reliability threshold, the decision result adopts the joint angle correction amount. If the confidence probability is lower than the reliability threshold, the decision result is the weighted average of the initial adjustment amount and the joint angle correction amount, and the weight is the confidence probability. The entire weighted fusion and decision process must be completed within the preset decision time limit, and the final adjustment angle command of the pitch joint and yaw joint is output.

5. The intelligent control system for spatial gripping posture of an orchard harvesting robot according to claim 4, characterized in that: The attitude arbitration decision-making unit performs weighted fusion and adjudication within a preset decision time limit, specifically including: The attitude arbitration decision unit has a preset decision cycle counter, which starts timing and counting when the initial clamping attitude and the spatial axis vector are received. In each decision cycle, the attitude arbitration decision unit reads the latest confidence probability and determines whether the reliability threshold or the upper limit of the number of decision cycles has been reached. If either condition is met, the cycle loop is terminated, and the final adjustment angle command is output based on the weighted result calculated in the current cycle. If the confidence probability has exceeded the reliability threshold before the upper limit of the number of cycles is reached, the joint angle correction amount of the corresponding cycle is used as the output.

6. The intelligent control system for spatial gripping posture of an orchard harvesting robot according to claim 1, characterized in that: The adjustment execution control unit drives the pitch and yaw joints according to the adjustment angle command, specifically including: The adjustment execution control unit receives an adjustment angle command from the attitude arbitration decision unit. The adjustment angle command includes the target angle of the pitch joint and the target angle of the yaw joint. The adjustment execution control unit reads the current actual angles of the pitch joint and the yaw joint, and calculates the difference between them and the corresponding target angles. The adjustment execution control unit employs a predictive control algorithm based on a known dynamic model of the manipulator. Using the difference as input, the algorithm predicts the overshoot and oscillations that will occur when driving the joint movement in subsequent control cycles. Based on the prediction results, it performs reverse optimization to generate a joint angle control sequence. This sequence allows the joint to transition from its current actual angle to the target angle. Throughout the adjustment process, the changes in velocity and acceleration are constrained. Finally, the adjustment execution control unit converts the joint angle control sequence into drive signals and sends them to the servo drivers of the pitch and yaw joints for execution in a time-division multiplexing manner.

7. The intelligent control system for spatial gripping posture of an orchard harvesting robot according to claim 1, characterized in that: In the light strip analysis module, the spatial axis vectors of the multiple local three-dimensional contour lines reconstructed in chronological order are filtered to suppress vector jitter. In the adjustment execution control unit, the real-time joint desired angle calculated according to the joint angle control sequence is filtered in the same way but with different parameters to smooth the drive command. An adaptive Kalman filter algorithm is introduced, and the process noise covariance matrix of the adaptive Kalman filter algorithm is dynamically adjusted according to the historical confidence probability or the actual joint motion error.

8. A method for implementing an intelligent control system for the spatial gripping posture of an orchard harvesting robot as described in any one of claims 1-7, characterized in that: Includes the following steps: S1. When the distance between the end effector and the target exceeds the activation threshold, the scene point cloud containing the target and the connecting part is acquired through the binocular vision module. The scene point cloud is segmented to obtain the connecting part point cloud cluster. The main direction vector of the connecting part point cloud cluster is calculated as the coarse estimation axis vector. Based on the coarse estimation axis vector, the initial adjustment amount of the pitch joint and yaw joint that makes the preset standard working plane normal vector of the end effector perpendicular to it is calculated. The spatial attitude of the end effector corresponding to the initial adjustment amount is determined as the initial clamping attitude. S2. When the end effector enters the near-field region within the activation threshold of the target object, a measurement is triggered. A structured light spot with an coded pattern is projected onto the connection region of the target object by a light spot projector fixed to the mechanical wrist. A high-speed macro camera acquires an image sequence of the non-Lambertian reflective light band formed by the structured light spot on the surface of the connection. The center line of the light band in each frame is extracted according to the preliminary adjustment amount, and the displacement and deformation of the center line of the light band are tracked between consecutive frames. Based on the continuous deformation trajectory of the center line of the light band, the local three-dimensional contour line of the surface of the connection in the light band illumination area is reconstructed in reverse through the triangulation principle. The local three-dimensional contour line is fitted with a spatial straight line to obtain the spatial axis vector of the connection, and a confidence probability characterizing the reliability of the spatial axis vector solution result is generated simultaneously. S3. Receive the initial clamping posture and its corresponding preliminary adjustment amount, as well as the spatial axis vector and its corresponding confidence probability. Transform the spatial axis vector into the robot's base coordinate system and calculate the joint angle correction amount required to make the normal vector of the end effector's working plane perpendicular to the vector. Calculate the posture deviation between the preliminary adjustment amount and the joint angle correction amount. Using the confidence probability as a weight, within a preset decision time limit, perform weighted fusion and adjudication on the adjustment tendency based on the preliminary adjustment amount and the adjustment tendency based on the joint angle correction amount to generate the final adjustment angle commands for the pitch and yaw joints. S4. In the final stage before the end effector executes the target action, the current actual angles of the pitch and yaw joints are read according to the adjustment angle command, the difference between the angles and the target angles is calculated, and a predictive control algorithm is used based on the manipulator dynamics model to predict the motion state in subsequent control cycles using the difference as input. The joint angle control sequence is then generated by reverse optimization, and the joint angle control sequence is converted into a drive signal to drive the pitch and yaw joints to move, so that the working plane of the end effector and the spatial axis vector satisfy the preset perpendicularity relationship, thus completing the closed-loop intelligent control of the spatial clamping posture.

Images