Control method of picking robot

Abstract

The invention discloses a control method of a picking robot, and particularly relates to the technical field of intelligent control. According to the method, three-dimensional point cloud data of all fruits in a fruit cluster area are collected in real time, and a spatial connection topological structure of fruit stems is established; establishing a finite element mechanical analysis model based on the spatial connection topological structure of the carpopodium, analyzing a stress transmission path after the picking action acts on the carpopodium, and predicting position deviation of adjacent unpicked fruits; meanwhile, a multi-target path optimization algorithm is adopted to generate an initial path of continuous picking of the picking robot, and then the spatial position of the next fruit picking action is dynamically corrected according to fruit stem stress transmission path data; and abnormal change characteristics of fruit positions and postures are identified according to actual position deviation data of the picking action, so that control parameters and path planning of the next picking action are adjusted in real time. The precision and stability of picking path planning are improved, and the overall efficiency and reliability of picking operation of the robot are improved.

Description

Technical Field

[0001] The present invention relates to the field of intelligent control technology, and more particularly, to a control method for a picking robot. Background Art

[0002] With the rapid development of agricultural automation technology, picking robots are widely used in fruit picking operations in orchards.

[0003] When existing picking robots continuously pick fruits, the torque applied to the fruit stalks by the previous picking action is easily transmitted to the adjacent fruit stalks through the fruit stalk connection structure and produces a fruit stalk interference effect, causing the adjacent unpicked fruits to be displaced and the posture to twist, causing the actual fruit position of the next picking action to deviate from the expected initial position, resulting in the picking robot failing to grasp or damaging the fruit during continuous operation, affecting the stability and efficiency of the robot's picking operation. Summary of the Invention

[0004] In order to overcome the above-mentioned defects of the prior art, an embodiment of the present invention provides a control method for a picking robot to solve the problems raised in the above-mentioned background technology.

[0005] To achieve the above object, the present invention provides the following technical solutions: A control method for a picking robot comprises the following steps: S1: Collect the 3D point cloud data of all fruits in the fruit cluster area in real time, analyze the spatial position relationship of the fruits and the connection structure of the fruit stalks, and output the spatial connection structure data of all fruit stalks in the fruit cluster; S2: Based on the spatial connection structure data of all fruit stalks in the fruit cluster, the stress transmission path after the picking action acts on the target fruit stalk is analyzed, the position offset caused by the adjacent unpicked fruits is predicted, and the fruit stalk stress transmission path data is output; S3: Based on the spatial connection structure data of all fruit stalks in the fruit cluster, the initial path sequence for the picking robot to continuously pick fruits is planned and the initial picking path of the robot is output; S4: Based on the fruit stalk stress transfer path data and the robot's initial picking path, the spatial position of the next fruit picking action is dynamically corrected, and the corrected robot picking path is output; S5: Based on the actual position deviation data after each picking action of the robot, identify the abnormal change characteristics of the fruit position and posture, and output the abnormal change characteristic data of the fruit position and posture; S6: Based on the abnormal change feature data and the corrected robot picking path, the control parameters and path planning of the robot's next picking action are adjusted in real time.

[0006] In a preferred embodiment, S1 is specifically: Collect three-dimensional point cloud data of all fruits in the fruit cluster area in real time, and perform preprocessing to obtain preprocessed three-dimensional point cloud data; Based on the pre-processed 3D point cloud data, the spatial position and geometric shape of the fruits in the fruit cluster area are identified, and the spatial position relationship between the fruits in the fruit cluster area and the connection structure characteristics of the fruit stalk are analyzed; Based on the spatial position relationship between fruits in the fruit cluster area and the connection structure characteristics of the fruit stalks, the spatial connection topology structure between all fruit stalks in the fruit cluster area is established, and the spatial connection structure data of all fruit stalks in the fruit cluster area are output.

[0007] In a preferred embodiment, S2 is specifically: Based on the spatial connection structure data of all fruit stalks in the fruit cluster area, a finite element mechanical analysis model of the fruit stalk connection structure in the fruit cluster area was established; Based on the finite element mechanics analysis model, the transmission path of the torque and stress generated by the clamping mechanism acting on the fruit stalk of the target fruit when the picking robot picks the target fruit is analyzed; Based on the transmission path of torque and stress, the position offset of adjacent unpicked fruits caused by the stress transmission of the fruit stalk connection structure is predicted; According to the position offset of adjacent unpicked fruits, the fruit stalk stress transfer path data is output.

[0008] In a preferred embodiment, S3 is specifically: Based on the spatial connection structure data of all fruit stalks in the fruit cluster area, a multi-objective optimization function including the robot arm movement distance, robot arm posture change and position offset risk index is constructed. Under the constraints of the robot arm's joint motion range, the fruit space collision avoidance constraint and the continuous picking sequence constraint, a multi-objective path optimization algorithm is used to generate several candidate continuous picking path sequences. Calculate the comprehensive evaluation value of the multi-objective optimization function for each candidate continuous picking path sequence; The candidate continuous picking path sequence with the best comprehensive evaluation value is determined as the robot's initial picking path.

[0009] In a preferred embodiment, S4 is specifically: For the next target picking action in the robot's initial picking path, the spatial prediction offset of the corresponding target fruit is extracted from the fruit stalk stress transfer path data; The target grasping position of the next target fruit at the picking moment is calculated based on the spatial prediction offset, and the path correction value is generated; The robot's initial picking path is incrementally updated using the path correction value to form an updated path node sequence; The updated path node sequence is checked for continuity and reachability to generate a revised robot picking path that satisfies the kinematic constraints of the robotic arm.

[0010] In a preferred embodiment, S5 is specifically: Acquire the actual grasping posture data after the current target fruit is picked and the target grasping posture data recorded for the current target fruit in the corrected robot picking path; Calculate the three-dimensional position deviation vector and attitude angle deviation vector between the actual grasping posture data and the target grasping posture data to form the current position deviation data; Performing a differential operation on the current position deviation data and the spatial prediction offset of the current target fruit predicted in the fruit stalk stress transfer path data to obtain a fruit position error residual vector; Threshold discrimination and cluster analysis are performed on the residual vector of fruit position error to extract abnormal change features of fruit position and posture; The abnormal change characteristics and the current position deviation data are combined to generate abnormal change characteristic data of the fruit position and posture.

[0011] In a preferred embodiment, S6 is specifically: Based on the abnormal change characteristic data, the control parameter correction amount of the next target picking action is calculated; Update the position offset risk indicator weight in the multi-objective optimization function based on the control parameter correction amount; The updated multi-objective optimization function is used to dynamically replan the corrected robot picking path to obtain the adjusted robot picking path; Based on the control parameter correction amount and the adjusted robot picking path, the control parameters and path planning of the robot's next picking action are adjusted in real time.

[0012] The technical effects and advantages of the control method of a picking robot of the present invention are as follows: By acquiring real-time 3D point cloud data of all fruits in the cluster area and constructing a spatial connection topology of the fruit stalks, the spatial layout and mechanical connections between the fruits are accurately reflected. The positional offset of adjacent unpicked fruits is predicted based on the spatial connection structure of all fruit stalks in the cluster, enabling pre-compensation for micro-displacements caused by torque transmission during continuous picking. Multi-target path planning is performed using the spatial connection structure data of all fruit stalks in the cluster to generate an optimal initial picking sequence that takes into account movement distance, posture changes, and positional offset risks. The predicted offset is used to dynamically correct the picking path, reducing the grasping failure rate caused by the causal stalk interference effect. Abnormal feature extraction of actual grasping deviations allows identification of persistent or directional offsets. Finally, based on the abnormal change feature data and the corrected robot picking path, adaptive adjustment of the gripping force, approach speed, and picking sequence is achieved. This improves the stability and success rate of continuous picking operations, reduces fruit damage, and increases robot picking efficiency and fruit integrity. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 This is a schematic diagram of a control method for a picking robot according to the present invention; Figure 2 This is the data table of robot fruit picking motion control parameters and path planning. DETAILED DESCRIPTION

[0014] The following will provide a clear and complete description of the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0015] Example Figure 1 The present invention provides a control method for a picking robot, which includes the following steps: S1: Collect the 3D point cloud data of all fruits in the fruit cluster area in real time, analyze the spatial position relationship of the fruits and the connection structure of the fruit stalks, and output the spatial connection structure data of all fruit stalks in the fruit cluster; S2: Based on the spatial connection structure data of all fruit stalks in the fruit cluster, the stress transmission path after the picking action acts on the target fruit stalk is analyzed, the position offset caused by the adjacent unpicked fruits is predicted, and the fruit stalk stress transmission path data is output; S3: Based on the spatial connection structure data of all fruit stalks in the fruit cluster, the initial path sequence for the picking robot to continuously pick fruits is planned and the initial picking path of the robot is output; S4: Based on the fruit stalk stress transfer path data and the robot's initial picking path, the spatial position of the next fruit picking action is dynamically corrected, and the corrected robot picking path is output; S5: Based on the actual position deviation data after each picking action of the robot, identify the abnormal change characteristics of the fruit position and posture, and output the abnormal change characteristic data of the fruit position and posture; S6: Based on the abnormal change feature data and the corrected robot picking path, the control parameters and path planning of the robot's next picking action are adjusted in real time.

[0016] S1: Collect 3D point cloud data of all fruits in the fruit cluster area in real time, analyze the spatial position relationship of the fruits and the connection structure of the fruit stalks, and output the spatial connection structure data of all fruit stalks in the fruit cluster, including: Collect three-dimensional point cloud data of all fruits in the fruit cluster area in real time, and perform preprocessing to obtain preprocessed three-dimensional point cloud data; A fruit cluster area refers to the spatial region containing multiple fruits, fruit stalks, and adjacent branches. Three-dimensional point cloud data refers to a collection of discrete points with three-dimensional coordinates generated by a depth camera or laser ranging sensor. Each point contains Cartesian coordinates (X, Y, and Z) in meters or millimeters, along with fields such as surface normal, curvature, intensity, and timestamp. Real-time acquisition involves continuously acquiring 3D point cloud data at fixed intervals. This interval can be 33 milliseconds, 50 milliseconds, or 100 milliseconds, depending on the robot's motion speed and target scene lighting conditions. The sensor is mounted above the robot's end effector or near its base. The mounting angle is calibrated using a calibration plate. The calibration results provide the rotation matrix and translation vector between the sensor coordinate system and the robot's base coordinate system, which are used for coordinate unification. The preprocessing process includes time synchronization, outlier removal, noise filtering, voxel downsampling, region of interest cropping, and normal estimation. Time synchronization synchronizes point cloud frames within the same control cycle with the robot's encoder posture data to ensure consistent coordinate transformations. Outlier removal is done using a statistical method, based on the K-nearest-neighbor distance distribution of each point. A standard deviation multiple threshold is set, for example, with a K range of 20 to 50 and a standard deviation multiple threshold of 1.0 to 2.0, to remove points with abnormal distance distributions. Noise filtering can be performed using radius filtering, with a radius threshold of 10 mm to 30 mm and a minimum neighbor threshold of 5 to 10, to remove isolated points. Voxel downsampling uses a voxel grid method with a voxel side length of 2 mm to 10 mm to obtain a point cloud with uniform density, reducing computational complexity. Region of interest cropping is based on the fruit cluster bounding box, which is obtained through coarse segmentation. A 10% to 20% redundant boundary is retained within the cropping range to preserve the point cloud at the junction of the fruit stalk and the branch. Normal vector estimation is based on local neighborhood principal component analysis, with a neighborhood radius of 2 to 3 times the voxel side length, outputting the unit normal vector for each point. The preprocessed 3D point cloud data is output in a structured data format. The data structure includes a point coordinate matrix, a normal vector matrix, a curvature vector, a timestamp sequence, and a coordinate system identifier. The file format can be either a binary point cloud format or a custom cache format. For example, using an apple cluster as the object, a depth frame with a resolution of 1280×720 is acquired at a distance of one meter from the target. The voxel downsampling is set to a 5 mm voxel edge length, and the statistical outlier rejection parameters are set to K = 30 and the standard deviation multiple threshold = 1.5. This results in preprocessed 3D point cloud data with approximately 150,000 to 200,000 points per frame. The above data format, parameters, and coordinate definitions remain unchanged.

[0017] Based on the pre-processed 3D point cloud data, the spatial position and geometric shape of the fruits in the fruit cluster area are identified, and the spatial position relationship between the fruits in the fruit cluster area and the connection structure characteristics of the fruit stalk are analyzed; Identifying the spatial position of the fruit means determining the geometric centroid position of each fruit and the position of the grasping reference point. The recognition process includes point cloud segmentation, target clustering and geometric fitting. Point cloud segmentation uses a clustering method based on geometric continuity to perform Euclidean clustering on the pre-processed three-dimensional point cloud data. The clustering distance threshold is set to 10 mm to 20 mm according to the voxel side length, the minimum cluster point number is set to 500 to 1,000 points, and the maximum cluster point number is set to within 200,000 points to separate individual fruits and branches. After the target clustering is completed, the geometric shape is determined for each candidate cluster. The geometric shape uses sphere fitting or ellipsoid fitting. The fitting method uses parameter fitting in the sense of least squares, and outputs the sphere radius or ellipsoid major and minor axis parameters, fitting residuals and circumscribed axis aligned bounding box parameters. The spatial position uses the fitted centroid as the spatial position of the fruit, and the geometric shape uses the fitting parameters as the fruit shape parameters. In order to ensure the connection with the fruit stalk analysis, it is necessary to identify the fruit stalk connection area in the fruit cluster point cloud. The pedicel connection structure features refer to the geometric and topological properties of the slender cylindrical or conical structure connecting the fruit to the branch, as represented in the point cloud. These include the pedicel axis direction vector, the coordinates of the pedicel's base endpoint, the local curvature distribution of the contact ring between the pedicel and the fruit, and the proximal diameter range of the pedicel. The pedicel axis direction vector is obtained by fitting a cylinder to a set of points with a radius of 20 to 40 mm below or to the sides of the fruit. The cylinder fitting radius ranges from 3 to 10 mm, with a minimum axial length threshold of 10 to 30 mm. The coordinates of the pedicel's base endpoint are obtained by extending the cylinder axis in the negative direction to the closest intersection with the branch point cloud. The spatial positional relationship between fruits is generated using two structures: the first is a Euclidean distance matrix between fruit centroids, where the matrix element d(i,j) represents the distance between the i-th and j-th fruits; the second is a set of relative orientation vectors of the fruits, where the vector v(i,j) points from the i-th fruit centroid to the j-th fruit centroid. After normalization, this vector is used to constrain the approach direction during path planning. For example, in a scene containing three adjacent apples, Euclidean clustering yields three fruit clusters and a set of branch clusters. The sphere fitting radius of each fruit cluster is approximately 45 to 55 mm. The cylindrical fitting radius of the fruit stem is approximately 3 to 5 mm, with the axis angled at 30 to 60 degrees to the direction of gravity. In the distance matrix, d(1,2) = 120 mm, d(2,3) = 95 mm, and d(1,3) = 210 mm. The set of relative orientation vectors is used to generate a set of candidate grasping approach directions. Through the above identification and analysis, four types of data are obtained: the spatial position of the fruit, its geometric shape, the spatial relationship between the fruits, and the structural characteristics of the fruit stem connection. A unified coordinate system identifier and timestamp are recorded in the data structure to ensure consistency with the finite element analysis input and the multi-objective path optimization input.

[0018] Based on the spatial position relationship between fruits in the fruit cluster area and the connection structure characteristics of the fruit stalks, the spatial connection topology structure between all fruit stalks in the fruit cluster area is established, and the spatial connection structure data of all fruit stalks in the fruit cluster area are output; The spatial connectivity topology refers to a graph structure that reflects the connectivity between fruit stalks and between fruit stalks and branches. The graph structure consists of a node set and an edge set. The node set consists of two types of nodes: the first type is the fruit stalk node, which records the stalk axis direction vector, the coordinates of the fruit stalk's base endpoint, the proximal diameter of the fruit stalk, an estimated stalk length, and the index of the fruit it contains. The second type is the branch connection node, which records the approximate direction of the branch centerline, the local radius of the branch, and its position in a unified coordinate system. The edge set represents two types of connectivity relationships: connectivity between a fruit stalk node and a branch connection node, representing the connection between the fruit stalk and the branch; and connectivity between multiple fruit stalk nodes under the same branch connection node, representing the association of fruit stalks on the same branch. The connectivity relationship construction rule is based on spatial proximity and axis geometric consistency. Edges are established when both a spatial distance threshold and an axis angle threshold are met. The spatial distance threshold is set when the closest distance from the fruit stalk's base endpoint to the branch point cloud is less than 15 mm, and the axis angle threshold is set when the angle between the fruit stalk axis and the local branch orientation is less than 30 degrees. The above rules ensure that the graph structure is consistent with the actual structure, avoiding incorrect connections. The spatial connectivity topology is stored simultaneously in the form of an adjacency list and an adjacency matrix. The adjacency list records the index of each node's connected nodes and the edge attributes. The edge attributes include the connection type identifier and the calculation time. The element A(p,q) of the adjacency matrix A takes a value of zero or one, with a value of one indicating that node p and node q are connected. In the engineering implementation, the spatial connectivity structure data is encapsulated using a unified data frame. The data frame fields include a node list, an edge list, an adjacency matrix, a timestamp, and a coordinate system identifier. For example, in a fruit cluster consisting of three apples and a branch, the graph structure includes three fruit stem nodes and a branch connection node. The edge list contains three edges, connecting the three fruit stem nodes to the branch connection node. The three rows and three columns of the adjacency matrix corresponding to the branch connection node have a value of one, and the remaining positions have a value of zero. To ensure data consistency, each fruit stem node in the spatial connectivity structure data stores the index corresponding to the fruit's spatial position and geometric shape in step S1, ensuring a consistent mapping from geometric identification to topological construction.

[0019] S2: Based on the spatial connection structure data of all fruit stems in the fruit cluster, the stress transfer path after the picking action acts on the target fruit stem is analyzed, the position offset caused by the adjacent unpicked fruits is predicted, and the fruit stem stress transfer path data is output, including: Based on the spatial connection structure data of all fruit stalks in the fruit cluster area, a finite element mechanical analysis model of the fruit stalk connection structure in the fruit cluster area was established; The spatial connectivity data for all fruit stalks within a cluster includes a node list, edge list, adjacency matrix, timestamp, and coordinate system identifier. The fruit stalk node in the node list contains the stalk axis direction vector, stalk base endpoint coordinates, stalk proximal diameter, estimated stalk length, and the fruit index. Branch connection nodes record the branch centerline direction, local radius, and position. The finite element mechanical analysis model is a digital simulation model established based on this spatial connectivity data. The finite element mechanical analysis model construction method includes model geometry construction, material property setting, meshing, and boundary condition definition. The model geometry is based on the spatial connectivity topology of the fruit stalk. Based on the stalk axis direction vector and stalk base endpoint coordinates, a three-dimensional geometric solid model of the stalk connection structure is constructed using a cylindrical or conical modeling approach. The base position of the cylinder or cone coincides with the stalk base endpoint coordinates, the axial length is the estimated stalk length, the base radius is half the stalk proximal diameter or a radius obtained from point cloud fitting, and the top radius is set based on the fruit contact ring diameter. The material property setting is based on the elastic and rigid characteristics of the actual fruit stalk. The fruit stalk is usually made of plant fiber material, with Young's modulus set to 50 MPa to 500 MPa, Poisson's ratio set to between 0.2 and 0.4, and density set to 800 to 1200 kg per cubic meter. The meshing is completed using the meshing method commonly used in finite element analysis. The mesh element type is hexahedron or tetrahedron unit, and the unit size is set to 0.5 mm to 2 mm. The boundary condition definition is a description of the fixed constraints and force loading conditions of the fruit stalk connection structure, in which the branch connection node is in a fixed constraint state to simulate the support and fixed conditions of the actual connection between the fruit stalk and the branch, and the connection between the end of the fruit stalk node and the fruit is set as the load-bearing loading surface. The definitions of the model geometric entities, material properties, meshing and boundary conditions are all recorded in the finite element mechanical analysis model data structure. For example, in a fruit cluster consisting of three apples, a finite element mechanical analysis model of the three fruit stalks connected was established. Each fruit stalk is a cylindrical structure with a diameter of 6 mm and a length of 40 mm. The Young's modulus of the material is 200 MPa, the Poisson's ratio is 0.3, the grid unit size is 1 mm, and the branch connection nodes are fixed in the spatial coordinate system.

[0020] Based on the finite element mechanics analysis model, the transmission path of the torque and stress generated by the clamping mechanism acting on the fruit stalk of the target fruit when the picking robot picks the target fruit is analyzed; Simulations analyze the stress and torque transmission paths within the fruit stem and its connecting structure when a harvesting robot's gripping mechanism applies mechanical force to the target fruit stem. The mechanical force applied by the gripping mechanism to the stem end is determined by the gripping mechanism's kinematic parameters. The force typically ranges from 0.5 Newtons to 5 Newtons, and its direction ranges from 10 degrees to 60 degrees along or perpendicular to the stem axis. This direction is dependent on the gripping posture of the robotic arm. A finite element mechanics analysis model applies the gripping force to the loading surface at the stem end. Static or dynamic simulations are used to determine the stress field distribution and torque transmission paths at the nodes and elements within the stem and its connecting structure. Stress field data includes the stress magnitude and direction for each element, while torque transmission path data includes the element-by-element path of stress transmission from the loading surface to the fixed node (branch connection node) within the stem, along with the corresponding torque changes. The transmission paths can be presented graphically or in a data table format, which records the stress and torque distribution from the loading surface to the branch connection node as they change spatially. For example, the picking robot applies a tensile force of 2 Newtons to the stalk of the target fruit, with the direction and the angle between the axial direction of the stalk and the finite element analysis showing that the maximum stress occurs at the connection between the bottom of the stalk and the branch, which is about 3 MPa. The torque is transmitted from the top of the stalk to the bottom node by node, and the stress gradually decreases along the length of the stalk.

[0021] Based on the transmission path of torque and stress, the position offset of adjacent unpicked fruits caused by the stress transmission of the fruit stalk connection structure is predicted; Based on the torque and stress transmission path data, the method analyzes and predicts the spatial position offset of adjacent fruits caused by mechanical stress on the stalks when the stress of the fruit stalk is transferred to the stalks of adjacent unpicked fruits. The stress transfer to the stalks of adjacent fruits causes slight deformation of the stalks, including changes in the length of the stalk axis and tilting of the stalk axis, which leads to changes in the spatial position and posture of the adjacent fruits. The method for predicting position offset includes calculating the change in the position of the stalk tip through linear interpolation based on simulation data, and calculating the distance and direction of the spatial position shift of the fruit center of gravity caused by the stalk tilt based on elastic deformation theory. The calculated position offset data includes a three-dimensional displacement vector and a change in posture angle. The three-dimensional displacement vector includes the displacement in the X, Y, and Z coordinate axes, measured in millimeters; the posture angle change includes the tilt angle of the stalk axis relative to the initial state, measured in degrees. For example, in the case of a cluster of three apples, after applying a tensile force to the first fruit, stress transfer path analysis predicts that the second fruit will experience a spatial offset of 2 mm along the X-axis and 1 mm along the Y-axis due to the transferred stress, and the inclination angle of the fruit stalk will increase by 5 degrees. The predicted position offset data is recorded in a data table for optimization of the picking control strategy.

[0022] Output the fruit stalk stress transfer path data based on the position offset of adjacent unpicked fruits; The predicted position offset data of adjacent unpicked fruits is output through a specific data structure as the fruit stem stress transfer path data, including the fruit number, three-dimensional position offset vector, attitude angle change, corresponding finite element analysis data record number and timestamp. This is used by the picking robot to make real-time corrections to the fruit grasping position and attitude during the next picking motion planning.

[0023] S3: Based on the spatial connection structure data of all fruit stems in the fruit cluster, the initial path sequence for the picking robot to continuously pick fruits is planned, and the robot's initial picking path is output, including: Based on the spatial connection structure data of all fruit stalks in the fruit cluster area, a multi-objective optimization function including the robot arm movement distance, robot arm posture change and position offset risk index is constructed. The spatial connectivity structure data for all fruit stalks within a cluster region includes a node list, edge list, adjacency matrix, timestamp, and coordinate system identifier. The node list records the axis direction vector, bottom endpoint coordinates, proximal diameter, length estimate, and fruit index of each fruit stalk node. The branch connection node records the branch centerline direction, local radius, and position. The edge list records the connection relationship between nodes. The adjacency matrix records the connection relationship between nodes in matrix form. The matrix elements are either zero or one, and one indicates that there is a connection relationship between the corresponding nodes. The multi-objective optimization function is a mathematical expression used to evaluate the continuous picking path of the picking robot. It contains multiple evaluation indicators, each of which is assigned a corresponding weight coefficient. The indicators are integrated through linear or nonlinear combination to obtain a comprehensive evaluation value of the path. The multi-objective optimization function includes three indicators: the robot arm movement distance, the robot arm posture change, and the position offset risk indicator. The arm movement distance refers to the length of travel required for the robot's end-effector to move from the current fruit position to the next, calculated using the Euclidean distance in three-dimensional space and expressed in millimeters. The arm posture change refers to the degree of change in posture of the end-effector when completing the task of picking two adjacent fruits. The posture is expressed in Euler angles or quaternions, and the change is calculated as the difference in posture angles, expressed in degrees. The position deviation risk index is derived from the stalk stress transfer path data and reflects the risk of position deviation due to mechanical force transmission. A higher position deviation risk index indicates a higher likelihood of position deviation during fruit picking. The position deviation risk index is derived from the position deviation vector amplitude recorded in the stalk stress transfer path data and the stability analysis of the stalk connection structure. The square of the deviation amplitude or an exponential function is used as the position deviation risk index. For example, when the predicted position deviation amplitude is 2 mm, the position deviation risk index is calculated as 4 (squared) or higher in exponential form to indicate a high risk situation. The multi-objective optimization function is expressed as follows: Objective Function Value = Weight Coefficient 1 × Robot Arm Movement Distance + Weight Coefficient 2 × Robot Arm Posture Change + Weight Coefficient 3 × Position Drift Risk Index. The weight coefficients are determined based on the robot's mission requirements. For example, when robot arm movement efficiency is the primary concern, Weight Coefficient 1 is 0.5, and Weight Coefficients 2 and 3 are each 0.25. If position drift risk control is the primary concern, Weight Coefficient 3 is 0.6, and Weight Coefficients 1 and 2 are each 0.2. The calculation results and weight coefficients for each indicator are preset before each picking task and are uniformly recorded and called up in the robot control system.

[0024] Under the constraints of the robot arm's joint motion range, the fruit space collision avoidance constraint and the continuous picking sequence constraint, a multi-objective path optimization algorithm is used to generate several candidate continuous picking path sequences. The arm joint motion range constraint refers to the range of motion angles for each joint of the harvesting robot, determined by the arm's hardware parameters. For example, the joint angle ranges for a six-axis arm are ±170 degrees, ±120 degrees, ±135 degrees, ±190 degrees, ±120 degrees, and ±360 degrees, respectively. The robot cannot move outside these ranges. The fruit space collision avoidance constraint prohibits the arm's end effector from colliding with other unpicked fruits in the fruit cluster during the picking process to avoid damage to the arm or the fruit. The continuous picking sequence constraint refers to the optimal continuous picking order between fruits. That is, after the current fruit is picked, the picking position of the next target fruit must consider the principles of minimizing the arm's motion distance, minimizing posture change, and minimizing the risk of position offset. The above constraints are reflected in the robot path optimization algorithm as mathematical inequalities or equality constraints, and all constraints must be met during robot motion path optimization.

[0025] The multi-objective path optimization algorithm is based on the constrained optimization problem composed of the above three constraints and the multi-objective optimization function. Through heuristic search or intelligent optimization methods, it gradually generates multiple feasible candidate continuous picking path sequences. Each candidate path sequence is an ordered set of several continuous fruit picking actions, and each fruit picking action records the grasping position and posture of the target fruit.

[0026] Genetic algorithm is used as the multi-objective path optimization algorithm, specifically: Define the chromosome encoding method. The chromosome encoding represents the candidate continuous picking path sequence of the picking robot. That is, the gene sequence of each chromosome corresponds to a fruit picking order, and the value at each gene locus represents the fruit number. For example, in a fruit cluster containing three apple fruits, the gene sequence [1,2,3] represents the picking order of fruit 1-fruit 2-fruit 3.

[0027] Determine the population initialization strategy. The population is the set of candidate solutions initially generated. The population size is typically 20 to 50 chromosomes, and 30 chromosomes are selected. The initial population is generated using a random sorting method. For example, the fruit to be picked in the cluster is randomly numbered to obtain the initial candidate picking order.

[0028] Set the fitness function, which is a multi-objective optimization function. The calculation method is: Fitness value = weight coefficient 1 × robot arm movement distance + weight coefficient 2 × robot arm posture change + weight coefficient 3 × position offset risk index; The weight coefficient 1, weight coefficient 2, and weight coefficient 3 are set to 0.5, 0.25, and 0.25 respectively; For each candidate path chromosome, the robot arm movement distance, robot arm posture change and position offset risk index between two adjacent fruits are calculated, and the comprehensive fitness value is calculated based on the weight coefficient. The smaller the fitness value, the better the path.

[0029] Definition of genetic operators, including selection operator, crossover operator and mutation operator; The selection operator adopts a roulette wheel method to select chromosomes with higher fitness to enter the next generation according to the fitness value; The crossover operator uses a partial matching crossover method. For example, two path chromosomes [1,2,3] and [3,1,2] are crossed, and the crossover segments are randomly selected and the gene positions within the segments are exchanged to generate a new path sequence. The mutation operator uses the exchange mutation method, that is, randomly selecting two gene sites in the chromosome, for example, mutating [1,2,3] to [2,1,3] to ensure diversity.

[0030] The algorithm termination condition is judged. The number of iterations of the genetic algorithm is usually set to 50 to 100 generations, for example, 80 generations. When the preset number of generations is reached, or the fitness value change is less than the preset threshold, the algorithm terminates and outputs the path chromosome with the smallest fitness value, that is, the optimal continuous picking path sequence.

[0031] Taking a cluster of three apples as an example, the genetic algorithm initially randomly generated multiple path chromosomes. After 80 generations of iterative calculations, the chromosome path sequence with the minimum overall fitness value was [1, 3, 2]. At this time, the total movement distance of the robot arm was 200 mm, the total posture change was 25 degrees, and the position deviation risk index was 3, which was better than other paths. Therefore, this chromosome sequence was determined as the initial robot picking path.

[0032] Calculate the comprehensive evaluation value of the multi-objective optimization function for each candidate continuous picking path sequence; For each candidate continuous picking path sequence, the entire path sequence is calculated based on the multi-objective optimization function. The robot arm movement distance, posture change, and position offset risk index are calculated for each continuous picking action in the path. The three index values ​​of each action are multiplied by the corresponding weight coefficient and summed to obtain the comprehensive evaluation value of the current candidate continuous picking path sequence. The smaller the comprehensive evaluation value, the better the path sequence. For example, for candidate path sequence 1, that is, fruit 1-fruit 2-fruit 3, the robot arm movement distance of the continuous action is calculated to be 120 mm and 95 mm, the posture change is 10 degrees and 8 degrees, and the position offset risk index is 2 and 1.5. The comprehensive evaluation value is obtained by combining the weight coefficient. All candidate path sequences are calculated in sequence, and the comprehensive evaluation value is recorded in the path sequence evaluation data table.

[0033] The candidate continuous picking path sequence with the best comprehensive evaluation value is determined as the robot's initial picking path; After calculating the comprehensive evaluation values ​​of all candidate continuous picking path sequences, the path sequence with the lowest comprehensive evaluation value is selected from the path sequence evaluation data table as the optimal path sequence. This optimal path sequence is then determined as the robot's initial picking path, serving as the initial execution sequence for the robot's actual picking operation. The robot's initial picking path records the robot's target spatial position and posture data for each fruit picking action. This data is used for robot motion control and path correction tasks during the actual picking operation. The data format is consistent with the path sequence evaluation data table.

[0034] S4: Based on the fruit stem stress transfer path data and the robot's initial picking path, the spatial position of the next fruit picking action is dynamically corrected, and the corrected robot picking path is output, including: For the next target picking action in the robot's initial picking path, the spatial prediction offset of the corresponding target fruit is extracted from the fruit stalk stress transfer path data; The robot's initial picking path records the target grasping posture of each fruit, including the three-dimensional coordinate values ​​of the target fruit's spatial position and the three-dimensional attitude angle data of the robot arm's end effector. The next target picking action of the robot's initial picking path represents the next fruit picking task to be performed after the current picking action is completed. The fruit stalk stress transfer path data includes the fruit number, three-dimensional vector data of the position offset, attitude angle change data, the corresponding finite element analysis data number, and a timestamp. It records the predicted spatial position and attitude offset between the fruit stalks due to mechanical forces. The predicted spatial offset is expressed as a three-dimensional displacement vector and an attitude angle offset. The three-dimensional displacement vector records the spatial position offset of the corresponding fruit due to the mechanical force applied to the adjacent fruit during the previous picking action, resulting from the transfer effect generated by the fruit stalk connection structure. It is expressed as coordinate offset values ​​in the X-axis, Y-axis, and Z-axis directions. The offset amplitude is in millimeters. The coordinate system definition is consistent with steps S1 and S2. The attitude angle offset is the deflection angle of the fruit axis from the original initial direction due to the torque transmission of the fruit stalk. It is recorded as a deflection angle with reference to the robot arm coordinate system and is measured in degrees. For example, if the next target picking action in the robot's initial picking path is to pick apple fruit numbered 2, then based on the predicted position offset of fruit 2 recorded in the fruit stalk stress transmission path data, the three-dimensional displacement vector and attitude angle offset are extracted. For example, the three-dimensional displacement vector is an offset of 2.0 mm in the X direction, 1.5 mm in the Y direction, and -0.5 mm in the Z direction; the attitude angle offset is a 3-degree clockwise deflection around the Y axis. The extracted spatial prediction offset data is used to calculate the robot's grasping posture correction.

[0035] The target grasping position of the next target fruit at the picking moment is calculated based on the spatial prediction offset, and the path correction value is generated; Based on the predicted spatial offset, the target grasping pose of the next target fruit, recorded in the initial picking path, is calculated to determine the target grasping pose at the actual picking moment. This is then used to correct the robot's picking path. The target grasping pose consists of position and attitude. The position is calculated by adding the original 3D coordinates of the target fruit to the predicted 3D displacement vector. This means the original target position coordinates are added to the predicted displacements in the X, Y, and Z directions to obtain the final adjusted grasping position coordinates. The attitude is calculated by adding the original 3D posture angle of the robotic arm end effector to the predicted posture angle offset. This is the direct addition of the original posture angle and the predicted deflection angle to generate the final grasping posture data. The path correction is the incremental value of the position and attitude corrections, respectively. The displacement correction is the 3D displacement vector data, and the attitude angle correction is the 3D angle deflection data. For example, for apple number 2, the original target grasping position is (300.0, 150.0, 120.0) mm, with an angle of (0, 90, 0) degrees. After adding the predicted displacement vector (2.0, 1.5, -0.5) mm and the angle deflection (0, 3, 0) degrees, the adjusted target grasping position is (302.0, 151.5, 119.5) mm, with an angle of (0, 93, 0) degrees. The path corrections are recorded as the displacement correction (2.0, 1.5, -0.5) mm and the angle correction (0, 3, 0) degrees. These path corrections are used to update the robot's initial picking path.

[0036] The robot's initial picking path is incrementally updated using the path correction value to form an updated path node sequence; The initial picking path node records the spatial position of the target fruit and the robot's arm posture for each successive picking action. Incremental updating involves adding calculated displacement corrections and attitude angle corrections to the target fruit node position and posture data, respectively, based on the original path node data to generate the corrected path node data. The update process involves adding the displacement corrections to the target node position coordinates in the robot's initial picking path, and adding the attitude angle corrections to the posture angle data, to generate updated path node sequence data. The updated path node sequence data records the actual spatial position coordinates of each node and the 3D posture data of the robot's end effector after the update. For example, for apple fruit number 2, the original position coordinates (300.0, 150.0, 120.0) mm are updated to (302.0, 151.5, 119.5) mm, and the original posture (0, 90, 0) degrees is updated to (0, 93, 0) degrees. This completes the incremental update and generates the corrected node data.

[0037] The updated path node sequence is checked for continuity and reachability to generate a revised robot picking path that satisfies the kinematic constraints of the manipulator. The updated path node sequence needs to be verified for continuity and reachability to ensure that the robot arm's motion path is smooth, continuous, and achievable when performing continuous movements. Continuity verification involves checking whether the robot arm's end effector exhibits sudden changes in motion between two adjacent path nodes, ensuring that the arm's position and posture changes are within a preset threshold. Reachability verification involves verifying whether the spatial position and posture of each path node are within the robot arm's workspace. Specifically, it involves verifying the arm's forward and inverse kinematics to ensure that all joints can reach the target node position and posture without exceeding the physical joint motion limits. Continuity verification is based on the difference between the positions and postures of two adjacent nodes. The position change threshold is set to no greater than the maximum step size of a single movement of the robot arm, for example, 50 mm, and the posture change threshold is set to no more than 10 degrees. Reachability verification performs kinematic calculations based on the robot arm's joint angle range to ensure that the kinematic solution for each node exists and that the joint angles meet the motion limits. For example, for a six-axis robot, the joint angles do not exceed the ±170 to ±360 degree limit. Once the path node sequence passes continuity and reachability checks, the output is a revised robot picking path that satisfies the robot's kinematic constraints. For example, after updating the nodes of an apple cluster, if both continuity and reachability checks are met, the final revised robot picking path node sequence is obtained and used for the picking operation.

[0038] S5: Based on the actual position deviation data after each picking action of the robot, identify the abnormal change characteristics of the fruit position and posture, and output the abnormal change characteristic data of the fruit position and posture, including: Acquire the actual grasping posture data after the current target fruit is picked and the target grasping posture data recorded for the current target fruit in the corrected robot picking path; The actual grasping pose data after completing the picking of the current target fruit refers to the actual position coordinates and attitude angles recorded by the robot's end-effector during the actual picking of the current fruit. This actual grasping pose data is measured and recorded in real time by high-precision encoders and attitude sensors installed on the end-effector. The actual grasping pose data includes the spatial coordinate data and attitude data of the end-effector during the actual grasping of the fruit. The spatial coordinate data consists of the position coordinates of the end-effector in a three-dimensional Cartesian coordinate system, measured by the arm's joint encoders, and includes the X, Y, and Z coordinates in millimeters. The attitude data is measured by the attitude sensor and expressed in the form of three-dimensional attitude angles, typically expressed in Euler angles, including the rotation angles around the coordinate system's X-axis, Y-axis, and Z-axis, in degrees. The actual grasping pose data is stored in the robot's control system as a structured data frame. The data frame fields include a timestamp, a coordinate system identifier, a position coordinate vector, and an attitude angle vector.

[0039] The target grasping posture data recorded for the current target fruit in the revised robot picking path refers to the theoretical values ​​of the grasping target position and posture for the current fruit recorded in the revised robot picking path node sequence. The target grasping posture data is calculated based on the fruit stalk stress transfer path data and the initial picking path, and includes the calculated and adjusted spatial position coordinates and posture angles. The spatial position coordinates use the same three-dimensional Cartesian coordinate system as the actual grasping posture data. The coordinate system definition is consistent with steps S1 and S2, and the data format is also consistent, including X, Y, and Z coordinates. The posture data is also represented by the three-dimensional posture angle of the robot arm end effector, including the rotation angle around the X axis, the rotation angle around the Y axis, and the rotation angle around the Z axis, in degrees. The target grasping posture data is also stored in the form of a structured data frame. The fields include the target fruit number, timestamp, coordinate system identifier, target position coordinates, and target posture angle data to ensure that the data format is completely consistent when compared with the actual grasping posture data.

[0040] For example, in an apple-picking robot scenario, the robot successfully grasps apple number 3. The high-precision position encoder mounted on the robot's end effector records the actual grasp position coordinates as (502.0, 304.0, 158.0) mm, while the posture sensor at the end of the robot records the posture data as 0 degrees rotation around the X-axis, 93 degrees rotation around the Y-axis, and 1 degree rotation around the Z-axis. In the corrected robot picking path node sequence, the calculated target grasp pose for apple number 3 is (500.0, 305.0, 160.0) mm, with posture angles of 0 degrees rotation around the X-axis, 90 degrees rotation around the Y-axis, and 0 degrees rotation around the Z-axis.

[0041] Calculate the three-dimensional position deviation vector and attitude angle deviation vector between the actual grasping posture data and the target grasping posture data to form the current position deviation data; The current position deviation data includes a three-dimensional position deviation vector and a posture angle deviation vector. The three-dimensional position deviation vector is obtained by calculating the difference between the actual grasping position coordinates and the target grasping position coordinates, specifically the actual grasping position coordinates minus the target grasping position coordinates. The result is expressed in millimeters, and the three dimensions correspond to the deviation values ​​of the X-axis, Y-axis, and Z-axis respectively. The posture angle deviation vector is obtained by calculating the difference between the actual grasping posture angle and the target grasping posture angle. Specifically, the actual posture angle minus the target posture angle. The three dimensions are the angle difference around the X-axis, the angle difference around the Y-axis, and the angle difference around the Z-axis, and the unit is degree. After the three-dimensional position deviation vector and the posture angle deviation vector are combined, a unified data structure is formed, namely the current position deviation data, which records the difference between the robot's actual picking behavior and the planned picking behavior. For example, for apple picking action number 3, the position coordinates recorded in the actual grasp pose data are (502.0, 304.0, 158.0) mm, while the position coordinates in the target grasp pose data are (500.0, 305.0, 160.0) mm. The calculated position deviation vector is (2.0, -1.0, -2.0) mm, indicating that the actual grasp position is offset by 2 mm in the X-axis, -1 mm in the Y-axis, and -2 mm in the Z-axis relative to the target position. Regarding the pose angle, the actual pose is (0 degrees, 93 degrees, 1 degree) and the target pose is (0 degrees, 90 degrees, 0 degrees). The calculated pose angle deviation vector is (0, 3, 1) degrees, indicating no deviation around the X-axis, 3 degrees around the Y-axis, and 1 degree around the Z-axis.

[0042] Performing a differential operation on the current position deviation data and the spatial prediction offset of the current target fruit predicted in the fruit stalk stress transfer path data to obtain a fruit position error residual vector; The current position deviation data represents the actual measurement error between the current actual grasping position and posture of the fruit and the target grasping position and posture. The spatial prediction offset recorded in the fruit stalk stress transfer path data represents the theoretical offset predicted based on the mechanical transfer effect between the fruit stalks in the fruit cluster area. The spatial prediction offset includes a position prediction offset vector and an attitude angle prediction offset vector. The position prediction offset vector records the predicted offset values ​​in the X-axis, Y-axis, and Z-axis directions, while the attitude angle prediction offset vector records the attitude angle prediction offset values ​​around the X-axis, Y-axis, and Z-axis directions. The differential operation refers to the vector difference calculation between the current position deviation data and the spatial prediction offset. The position data and attitude data are subtracted dimension by dimension to analyze the deviation between the actual error and the theoretical prediction error. The final result is called the fruit position error residual vector, which includes the position error residual vector and the attitude error residual vector.

[0043] The position error residual vector represents the remaining error after subtracting the theoretically predicted position deviation from the actual measured position deviation in the X-axis, Y-axis, and Z-axis directions, reflecting the position deviation caused by other interference factors in addition to the mechanical effect of the fruit stalk during the fruit picking process; the attitude error residual vector represents the remaining angle error after subtracting the theoretically predicted attitude deviation from the actual measured attitude angle deviation around the X-axis, Y-axis, and Z-axis directions, reflecting the additional deviation source introduced by the attitude angle during the actual grasping process.

[0044] The position error residual vector = current position deviation vector - predicted position offset vector; the attitude error residual vector = attitude angle deviation vector - predicted attitude angle deviation vector. For example, taking fruit numbered 3 in an apple cluster, the position deviation vector in the current position deviation data is (2.0, -1.0, -2.0) mm, and the attitude angle deviation vector is (0, 3, 1) degrees. The predicted position offset vector recorded for fruit numbered 3 in the stalk stress transfer path data is (1.5, -0.5, -1.0) mm, and the predicted attitude offset vector is (0, 2, 0) degrees. Therefore, the calculated position error residual vector is (0.5, -0.5, -1.0) mm, and the calculated attitude error residual vector is (0, 1, 1) degrees. These results are the fruit position error residual vector data.

[0045] Threshold discrimination and cluster analysis are performed on the residual vector of fruit position error to extract abnormal change features of fruit position and posture; The residual error vector for fruit position reflects the portion of the actual deviation in fruit position and posture that cannot be explained by the mechanical effects of the fruit stem. This may involve additional unknown interference factors or random perturbations. This residual error vector requires in-depth analysis using threshold discrimination and cluster analysis to extract features of abnormal changes in fruit position and posture, allowing adjustments to harvesting strategies.

[0046] Threshold determination involves setting and comparing the amplitude of the error residual vector. The amplitude is defined as the Euclidean distance or modulus between the three-dimensional position error residual vector and the attitude error residual vector. The threshold is set based on a statistical analysis of historical harvesting data to determine the normal distribution range of the error residual vector amplitude. For example, the amplitude threshold for the position error residual vector is set between 0.5 mm and 1.0 mm, and the amplitude threshold for the attitude error residual vector is set between 1 degree and 3 degrees. If the amplitude of the error residual vector exceeds the set threshold, it is considered an abnormal change in the harvesting action and recorded as abnormal change feature data.

[0047] Cluster analysis involves statistically analyzing the direction, amplitude, and duration of the error residual vector across several consecutive picking actions. Using a data clustering algorithm, such as K-means clustering, the frequency of abnormal errors with similar error directions and amplitudes, i.e., the duration, is calculated. The results of cluster analysis are characteristic parameters for abnormal changes in fruit position and posture, including the amplitude of the error residual vector, the direction vector, and the number of consecutive occurrences of the abnormal change across consecutive actions. The direction vector, represented by the normalized unit vector of the error residual vector, records the spatial or posture direction in which the error primarily occurs. The amplitude represents the magnitude of the error, and the duration represents the frequency of recurrence of similar error anomalies across several consecutive picking actions.

[0048] For example, in an apple picking scenario, when picking apples numbered 3 ten times in a row, if it is found that the amplitude of the residual vector of the position error is about 1.2 mm for eight times, exceeding the threshold of 1.0 mm, and the direction is concentrated in the positive direction of the X-axis, and the number of times it lasts reaches 8 times, it is considered that an abnormal spatial position change feature has occurred. The abnormal change feature parameters include an amplitude of 1.2 mm, a direction of the positive unit vector of the X-axis, and a number of times it lasts 8 times.

[0049] Combine the abnormal change characteristics with the current position deviation data to generate abnormal change characteristic data of the fruit position and posture; Abnormal change characteristics are recorded together with the current position deviation data in a unified data structure to form abnormal change characteristic data for the fruit's position and posture. This data includes: the target fruit number, current position deviation data (including position deviation vector and posture deviation vector), abnormal change characteristic amplitude, abnormal change characteristic directional unit vector, abnormal change characteristic duration count, timestamp, and coordinate system identifier. For example, for apple number 3, during the current picking action, the current position deviation vector is (2.0, -1.0, -2.0) mm, and the posture deviation vector is (0, 3, 1) degrees. After threshold discrimination and cluster analysis, the abnormal change characteristic amplitude is 1.2 mm, the directional unit vector is in the positive direction of the X-axis, and the duration count is 8. All of this information together forms a data frame for abnormal change characteristic data of the fruit's position and posture, which is used to correct control parameters and adjust the path in real time.

[0050] S6: Based on the abnormal change feature data and the corrected robot picking path, the control parameters and path planning of the robot's next picking action are adjusted in real time, including: Based on the abnormal change characteristic data, the control parameter correction amount of the next target picking action is calculated; Based on the abnormal change characteristic data, the control parameter correction amount of the next target picking action is calculated. That is, according to the degree and direction of the abnormal change, the execution parameters of the robot picking action are adjusted dynamically in real time to reduce the probability of abnormal changes in position and posture during the picking action, ensure the picking process is stable and reliable, and reduce the risk of damage to the fruit and the robotic arm.

[0051] The control parameter correction includes three control parameters: clamping force correction, approach speed correction and clamping time correction, which are determined according to the amplitude, direction and duration of abnormal change characteristic data, and are generated through function mapping, proportional relationship or table lookup.

[0052] The clamping force correction is calculated based on the amplitude and duration of the abnormal change feature. The initial clamping force is typically set between 0.5 Newtons and 5 Newtons. If the amplitude of the abnormal change feature is large and persists frequently, it indicates that insufficient or excessive gripping force is causing the fruit to shift or become abnormal during gripping. The gripping force should be increased or decreased based on the actual situation. The calculation method uses a set clamping force correction coefficient. If the amplitude of the abnormal change feature exceeds a set threshold and points in the positive X-axis direction or toward a weak area of ​​the stem, the clamping force is increased by 5% to 15%. If the amplitude indicates excessive gripping force, causing the fruit to deflect, the gripping force is reduced by 5% to 15%. For example, if, during apple picking action number 3, the amplitude of the abnormal change feature reaches 1.2 mm multiple times and points toward a weak area of ​​the stem, the clamping force is increased by 10%, or 0.2 Newton, from the initial setting of 2 Newtons, for a corrected clamping force of 2.2 Newtons.

[0053] The approach speed correction is determined based on the amplitude and direction of the abnormal change characteristic. The approach speed is usually initially set at 10 mm / s to 100 mm / s. If the amplitude of the abnormal change characteristic is large and the direction points to the direction of a large position deviation when the robot arm approaches the fruit, it indicates that the robot arm's approach speed may be too fast, causing a collision with the fruit or an abnormal position of the fruit stalk. In this case, the approach speed is reduced by 10% to 30%. If the fruit has a position deviation but the direction is unrelated to the approach speed, there is no need to correct the approach speed. For example, if the abnormal change characteristic indicates that the robot arm has collided with the fruit when approaching in 5 consecutive picking actions, the amplitude is 1.0 mm to 1.5 mm, and the direction is the approach direction of the robot arm, then the initial approach speed of 50 mm / s needs to be reduced by 20%. The corrected approach speed is 40 mm / s to reduce the probability of position abnormalities when approaching the fruit.

[0054] The clamping time correction is calculated based on the number of times the abnormal change characteristic persists and the characteristics of the posture deviation. The clamping time is usually set between 1 and 5 seconds. If the abnormal change characteristic persists for a long time and is mainly manifested in posture deviation, it means that the clamping time may be too short, and the fruit begins to move before it stabilizes, causing posture abnormality. Therefore, the clamping time should be increased by 10% to 25%. For example, if there are 7 consecutive abnormal posture deviations and the amplitude exceeds the threshold of 3 degrees, the clamping time will be increased by 15% from the initial 2 seconds. The corrected clamping time is 2.3 seconds to ensure the stability of the fruit.

[0055] When the abnormal change characteristic data indicates that it is necessary to reduce the approach speed and adjust the clamping force at the same time, first calculate the extension of the clamping action time caused by the reduction in approach speed, and analyze the stress response characteristics of the fruit stem during the extension time, so as to make corresponding secondary corrections to the clamping force: A coupling relationship model between the clamping force, approach speed and stalk stress transmission is established, specifically: First, the finite element method was used to simulate and analyze the stress distribution of the fruit stalk structure under different clamping forces and action times. The cumulative characteristic curve of the fruit stalk stress was obtained, and the change pattern of stress with action time under different clamping forces was determined. When the clamping force was small, the stress increased linearly or slowly with the action time, while when the clamping force was large, it showed a nonlinear accelerated growth trend. For example, under the conditions of a clamping force of 2 Newtons and an action time of 1 second, the maximum stress of the fruit stalk was 2 MPa. After the clamping time was extended to 1.5 seconds, the stress may reach 2.8 MPa, an increase of 40%. When the clamping force was 1.8 Newtons, the stress growth trend slowed down, and the stress may only increase to 2.4 MPa, an increase of 20%. The above rules are used to determine the sensitive relationship between the clamping force and the extension of the action time. In actual picking operations, when the robot's approach speed is reduced from the initial value (e.g., 50 mm / s) to the revised value (e.g., 40 mm / s), the time required for the robot arm's end effector to reach the gripping position from the safe distance will increase accordingly. For example, if the original approach distance is 100 mm, the initial time is 2 seconds, which increases to 2.5 seconds after the revision. As a result, the stable time window before the gripping action begins is shortened, and the gripping action must be prolonged to ensure stable gripping.

[0056] Based on the above coupling effect analysis, when it is determined that the approach speed needs to be reduced (for example, by 20%), the extension of the clamping force action time caused by the reduction is calculated (for example, by 0.5 seconds). Combined with the fruit stem stress characteristic curve, it is determined whether the clamping force needs to be fine-tuned. For example, assume that the abnormal change characteristic continuously shows an amplitude of 1.2 mm, points in the positive direction of the X-axis, and the robot arm approaches too fast, causing an abnormal collision with the fruit. The initial gripping force is 2 Newtons, and the initial approach speed is 50 mm / s.

[0057] If the approach speed is corrected alone, reducing it by 20% to 40 mm / s would result in an extension of the clamping duration by 0.5 seconds (from 2 seconds to 2.5 seconds). Based on the aforementioned stalk stress characteristic curve, while the clamping force remains at 2 Newtons, the stress increases from 2 MPa to 2.8 MPa. This excessive increase would lead to a more serious risk of stalk stress transmission. Therefore, a secondary correction of the clamping force, reducing it by 10% (i.e., by 0.2 Newtons, resulting in a corrected clamping force of 1.8 Newtons), would be considered. This reduction in clamping force would reduce the stress increase from 40% to 20%, effectively mitigating the cumulative stress effect caused by the extended clamping duration.

[0058] The control parameter correction data is formed by the calculated clamping force correction, approach speed correction and clamping time correction, and is uniformly recorded in the control parameter correction data frame. The data frame fields include the target fruit number, clamping force correction (Newton), approach speed correction (mm per second), clamping time correction (seconds), timestamp and coordinate system identification.

[0059] Update the position offset risk indicator weight in the multi-objective optimization function based on the control parameter correction amount; The multi-objective optimization function is a mathematical expression used to evaluate the continuous picking path of the picking robot. It contains three indicators: the movement distance of the robotic arm, the change in the robotic arm posture, and the position offset risk index. Each indicator corresponds to a certain weight coefficient, and the indicators are linearly or nonlinearly combined to form a comprehensive evaluation value of the path.

[0060] The position shift risk indicator indicates the risk of position shift during fruit picking due to mechanical transmission effects on the fruit stem. The initial value of the original weight coefficient is set between 0.2 and 0.6. The initial value of the original weight coefficient is determined based on the different sensitivities of different fruit characteristics such as hardness, fruit stem length, fruit stem stiffness, and fruit cluster density to position shift risk in the picking scenario: Regarding fruit hardness, the lower the fruit hardness, the more sensitive it is to the clamping force on the fruit stem. The more pronounced the stress transfer effect on the fruit stem during the clamping process, the greater the risk of positional shift. Therefore, the initial weight coefficient of the positional shift risk indicator should be set within a relatively high range (e.g., 0.4 to 0.6). For example, when targeting fruits with lower hardness, such as grapes or strawberries, the initial weight coefficient of the positional shift risk indicator can be set to 0.5 to fully reflect the positional shift risk caused by the clamping action. For fruits with higher hardness, such as apples or pears, the initial weight coefficient of the positional shift risk indicator can be set lower, such as between 0.2 and 0.3.

[0061] In terms of fruit stalk length, longer fruit stalks usually have greater degrees of freedom, the stress transfer path generated after mechanical force acts is complex and the fruit position deviation is large, so the weight coefficient of the position deviation risk indicator should be increased; for apple fruit stalks with a length of more than 30 mm, the weight coefficient of the position deviation risk indicator can be set to 0.4 to 0.6; for fruits with shorter fruit stalks (such as those with a length of less than 20 mm), the weight coefficient of the position deviation risk indicator can be reduced to between 0.2 and 0.3.

[0062] Regarding the stiffness of the fruit stalk, fruit stalks with lower stiffness are prone to deformation during the clamping process, resulting in positional displacement. Therefore, the weight coefficient of such fruits needs to be appropriately increased (such as 0.5 to 0.6). On the contrary, fruit stalks with greater stiffness have a strong ability to resist deformation and a lower risk of positional displacement. Therefore, the weight coefficient can be set in a lower range (such as 0.2 to 0.3).

[0063] As for fruit cluster density, the higher the cluster density, the smaller the spatial distance between adjacent fruit positions. Positional deviations caused by the robot arm's movements are more likely to cause a chain reaction, leading to the risk of continuous positional deviations. Therefore, the positional deviation risk indicator weight coefficient for high-density fruit clusters (such as grape or cherry tomato clusters) should be appropriately increased to 0.4 to 0.6. For more dispersed fruit clusters (such as sparsely arranged apple clusters), the weight coefficient can be reduced to 0.2 to 0.3.

[0064] Taking a real-world scenario as an example, when picking strawberries, the risk of positional shift is extremely high due to their low hardness, slender and soft fruit stalks, and high density of fruit clusters. Therefore, the weight coefficient of the positional shift risk indicator should be set to 0.6. When picking apples, their hardness is high, the fruit stalks are moderately rigid, the fruit stalks are short, and the fruit clusters are relatively dispersed. Therefore, the weight coefficient of the positional shift risk indicator can be set to 0.25.

[0065] The position offset risk index weight is updated based on the control parameter correction. Specifically, when the clamping force correction, approach speed correction or clamping time correction in the control parameter correction exceeds the preset threshold, it indicates that the current position offset risk is high or low. The position offset risk index weight should be adjusted dynamically in real time to reflect the changes in the fruit position offset risk during the actual picking process and optimize the robot path planning.

[0066] The updating method maps the clamping force correction, approach speed correction, and clamping time correction to the position drift risk indicator weight using proportional coefficients. For example, for every 0.1 Newton increase in the clamping force correction, the position drift risk indicator weight increases by 0.05; for every 5 mm / s decrease in the approach speed correction, the weight increases by 0.05; and for every 0.1 second increase in the clamping time correction, the weight increases by 0.05. For apple number 3, the clamping force increases by 0.2 Newtons (weight increase of 0.1), the approach speed decreases by 10 mm / s (weight increase of 0.1), and the clamping time increases by 0.3 seconds (weight increase of 0.15), resulting in a total weight increase of 0.35. The original position drift risk indicator weight is 0.25, so the updated position drift risk indicator weight is 0.6, reflecting the increased risk of fruit position drift.

[0067] The updated results of the weight coefficients are recorded in real time in the multi-objective optimization function parameter data table. The data table includes the weight coefficient of the position offset risk indicator, the weight coefficient of the robot arm movement distance, the weight coefficient of the robot arm posture change, the update timestamp, and the fruit number, to ensure that the updated multi-objective optimization function is used for real-time dynamic replanning and execution of the robot picking path.

[0068] The updated multi-objective optimization function is used to dynamically replan the corrected robot picking path to obtain the adjusted robot picking path; Dynamic replanning first extracts information about the unexecuted picking path nodes in the revised robot picking path, including the nodes' spatial coordinates, attitude angle data, and the corresponding fruit numbers. The path nodes are then reordered and optimized by re-executing the path optimization algorithm. The algorithm performs the following steps: A comprehensive evaluation value of the multi-objective optimization function is calculated for each unpicked path node. The updated weight coefficients for the position offset risk indicator, the robot's movement distance, and the attitude change weight coefficients are used to calculate the index value for each node. The index value is then combined with the path planning results between adjacent nodes to determine the overall optimality of the node sequence. For example, the dynamic replanning process is illustrated using the path nodes for unpicked fruit numbers 4, 5, and 6 in an apple cluster. In the revised path node sequence, the spatial coordinates of fruit number 4 are (400.0, 250.0, 100.0) mm, fruit number 5 is (420.0, 240.0, 110.0) mm, and fruit number 6 is (430.0, 255.0, 105.0) mm. The initial revised picking order is fruit number 4-fruit number 5-fruit number 6. The weight coefficient of the position deviation risk indicator has been updated from 0.25 to 0.6, and the weight coefficients for the robot arm's movement distance and posture change are each 0.2. The path optimization algorithm recalculates the indicators for each path. For example, starting from the current robot end-effector position of (380.0, 240.0, 120.0) mm, the robot arm's movement distance, posture change, and position deviation risk indicator for fruit numbers 4, 5, and 6 are calculated, respectively. A comprehensive evaluation value for each fruit node is calculated based on the updated weights. Assume that the calculation result is: the total comprehensive evaluation value corresponding to the initial sequence of No. 4-No. 5-No. 6 is 80, while the total comprehensive evaluation value of the sequence No. 5-No. 6-No. 4 is 60, which is smaller. Therefore, the new node picking order is determined to be No. 5-No. 6-No. 4. Through dynamic replanning in this way, the adjusted robot picking path has the minimum position deviation risk, the most reasonable robot arm movement distance, and the optimal robot arm posture change characteristics.

[0069] The replanned path results are recorded in the updated picking path data table, which includes the path node numbers, the optimized picking sequence, the target space coordinates corresponding to each node, the posture data of the end effector of the robotic arm, and the comprehensive evaluation value after path optimization, so that the robot control system can call and execute it in real time.

[0070] Based on the control parameter correction amount and the adjusted robot picking path, the control parameters and path planning of the robot's next picking action are adjusted in real time; The robot's adjusted picking path and control parameter corrections obtained through dynamic replanning are used to adjust the robot's next picking action in real time. These control parameter corrections include gripping force, approach speed, and gripping time corrections. The adjusted picking path determines the spatial coordinate position and attitude angle of the target fruit for the next picking action.

[0071] Real-time adjustment methods include real-time adjustment of control parameters and real-time fine-tuning of path planning. Real-time adjustment of control parameters means that the robot control system updates the clamping force, approach speed, and clamping time based on the data of the control parameter correction amount. The real-time adjustment of the clamping force is completed by the force feedback control unit inside the robot's clamping mechanism. It measures the force on the fruit handle in real time during the clamping process and dynamically adjusts the actual clamping force output by the actuator to ensure that the actual execution value of the clamping force accurately matches the corrected target value. For example, taking apple number 5 as an example, the initial clamping force is 2 Newtons, and the clamping force correction amount is an increase of 0.2 Newtons. During the real-time adjustment process, the clamping actuator continuously adjusts the actual output torque based on the real-time measurement data of the force feedback sensor to maintain the actual clamping force value within the set range of approximately 2.2 Newtons.

[0072] Real-time approach speed adjustment relies on the robot's speed controller to dynamically adjust the motor drive current or control voltage, ensuring that the actual speed of the robot's end effector as it approaches the fruit matches the corrected speed setting. Real-time gripping time adjustment uses a timing unit within the robot's control system to set the hold time of the gripping action in real time, precisely controlling the duration of the gripping action.

[0073] Real-time adjustment of path planning refers to the robot picking control system performing path tracking and fine-tuning control based on the adjusted robot picking path. The real-time adjustment method includes the path tracking controller performing real-time error compensation based on the actual execution position measured in real time by the position feedback sensor and the adjusted path planning position, and fine-tuning the position and speed of each joint of the robotic arm to ensure that the actual motion path accurately coincides with the planned path and eliminate path execution deviations. For example, when actually picking apple number 5, the real-time feedback of the actual position of the end of the robotic arm is (419.0, 240.5, 109.5) mm, which is slightly different from the planned position (420.0, 240.0, 110.0) mm. After calculating the position deviation, the real-time path tracking controller sends fine-tuning instructions to each joint motor to make slight adjustments, and corrects the end effector position to the planned position in real time, thereby achieving precise path execution.

[0074] The above real-time adjustment data is recorded in the real-time control log data table in the robot control system. The data table includes the real-time adjusted clamping force, approach speed, clamping time, and the real-time execution position coordinates and posture angle of the robot picking path, ensuring that the robot's picking action is stable, efficient, accurate and reliable.

[0075] In order to more clearly and completely reflect the data association and logical coherence between the steps involved in the control method of a picking robot of the present invention, the following data table ( Figure 2 All key data involved in the robot's fruit picking action is publicly available in the form of a data table (data table of control parameters and path planning for robot fruit picking actions). The meaning of each column in the data table is as follows: Picking action number: indicates the order of consecutive picking actions performed by the robot; Target fruit number: indicates the target fruit number for each picking; Initial position coordinates and posture angles: represent the target grasping position and posture calculated by the robot during initial path planning; Corrected position coordinates and posture angles: represent the target grasping position and posture that are dynamically corrected based on the calculation of the fruit stalk stress transfer path data; Clamping force, approach speed and clamping time: control parameters that are corrected in real time based on abnormal change characteristic data.

[0076] The above formulas are all dimensionless and numerical calculations. The formulas are obtained by collecting a large amount of data and performing software simulation to obtain the most recent real situation. The preset parameters and thresholds in the formulas are set by technicians in this field according to actual conditions.

[0077] The above embodiments can be implemented in whole or in part via software, hardware, firmware, or any other combination. When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product comprises one or more computer instructions or computer programs. When loaded or executed on a computer, the processes or functions described in the embodiments of this application are fully or partially performed. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired means (e.g., infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium accessible by a computer or a data storage device such as a server or data center that contains a collection of one or more available media. The available medium can be magnetic media (e.g., floppy disks, hard disks, tapes), optical media (e.g., DVDs), or semiconductor media. The semiconductor media can be a solid-state drive.

[0078] Those skilled in the art will appreciate that the modules and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0079] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and modules described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0080] In the several embodiments provided in this application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are merely schematic. For example, the division of the modules is only a logical function division. In actual implementation, there may be other division methods, such as multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or modules, which can be electrical, mechanical or other forms.

[0081] The modules described as separate components may or may not be physically separate, and the components shown as modules may or may not be physical modules, and may be located in one place or distributed across multiple network modules. Some or all of the modules may be selected to achieve the purpose of this embodiment according to actual needs.

[0082] In addition, each functional module in each embodiment of the present application may be integrated into one processing module, or each module may exist physically separately, or two or more modules may be integrated into one module.

[0083] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0084] The above description is merely a specific embodiment of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of this application. Therefore, the scope of protection of this application should be based on the scope of protection of the claims.

[0085] Finally: The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A control method for a picking robot, characterized in that: The steps include: S1: Collect the 3D point cloud data of all fruits in the fruit cluster area in real time, analyze the spatial position relationship of the fruits and the connection structure of the fruit stalks, and output the spatial connection structure data of all fruit stalks in the fruit cluster; S2: Based on the spatial connection structure data of all fruit stalks in the fruit cluster, the stress transmission path after the picking action acts on the target fruit stalk is analyzed, the position offset caused by the adjacent unpicked fruits is predicted, and the fruit stalk stress transmission path data is output; S3: Based on the spatial connection structure data of all fruit stalks in the fruit cluster, the initial path sequence for the picking robot to continuously pick fruits is planned and the initial picking path of the robot is output; S4: Based on the fruit stalk stress transfer path data and the robot's initial picking path, the spatial position of the next fruit picking action is dynamically corrected, and the corrected robot picking path is output; S5: Based on the actual position deviation data after each picking action of the robot, identify the abnormal change characteristics of the fruit position and posture, and output the abnormal change characteristic data of the fruit position and posture; S6: Based on the abnormal change feature data and the corrected robot picking path, the control parameters and path planning of the robot's next picking action are adjusted in real time.

2. The control method of a picking robot according to claim 1, characterized in that: S1, specifically: Collect three-dimensional point cloud data of all fruits in the fruit cluster area in real time, and perform preprocessing to obtain preprocessed three-dimensional point cloud data; Based on the pre-processed 3D point cloud data, the spatial position and geometric shape of the fruits in the fruit cluster area are identified, and the spatial position relationship between the fruits in the fruit cluster area and the connection structure characteristics of the fruit stalk are analyzed; Based on the spatial position relationship between fruits in the fruit cluster area and the connection structure characteristics of the fruit stalks, the spatial connection topology structure between all fruit stalks in the fruit cluster area is established, and the spatial connection structure data of all fruit stalks in the fruit cluster area are output.

3. The control method of a picking robot according to claim 2, characterized in that: S2, specifically: Based on the spatial connection structure data of all fruit stalks in the fruit cluster area, a finite element mechanical analysis model of the fruit stalk connection structure in the fruit cluster area was established; Based on the finite element mechanics analysis model, the transmission path of the torque and stress generated by the clamping mechanism acting on the fruit stalk of the target fruit when the picking robot picks the target fruit is analyzed; Based on the transmission path of torque and stress, the position offset of adjacent unpicked fruits caused by the stress transmission of the fruit stalk connection structure is predicted; According to the position offset of adjacent unpicked fruits, the fruit stalk stress transfer path data is output.

4. The control method of a picking robot according to claim 3, characterized in that: S3, specifically: Based on the spatial connection structure data of all fruit stalks in the fruit cluster area, a multi-objective optimization function including the robot arm movement distance, robot arm posture change and position offset risk index is constructed. Under the constraints of the robot arm's joint motion range, the fruit space collision avoidance constraint and the continuous picking sequence constraint, a multi-objective path optimization algorithm is used to generate several candidate continuous picking path sequences. Calculate the comprehensive evaluation value of the multi-objective optimization function for each candidate continuous picking path sequence; The candidate continuous picking path sequence with the best comprehensive evaluation value is determined as the robot's initial picking path.

5. The control method of a picking robot according to claim 4, characterized in that: S4, specifically: For the next target picking action in the robot's initial picking path, the spatial prediction offset of the corresponding target fruit is extracted from the fruit stalk stress transfer path data; The target grasping position of the next target fruit at the picking moment is calculated based on the spatial prediction offset, and the path correction value is generated; The robot's initial picking path is incrementally updated using the path correction value to form an updated path node sequence; The updated path node sequence is checked for continuity and reachability to generate a revised robot picking path that satisfies the kinematic constraints of the robotic arm.

6. The control method of a picking robot according to claim 5, characterized in that: S5, specifically: Acquire the actual grasping posture data after the current target fruit is picked and the target grasping posture data recorded for the current target fruit in the corrected robot picking path; Calculate the three-dimensional position deviation vector and attitude angle deviation vector between the actual grasping posture data and the target grasping posture data to form the current position deviation data; Performing a differential operation on the current position deviation data and the spatial prediction offset of the current target fruit predicted in the fruit stalk stress transfer path data to obtain a fruit position error residual vector; Threshold discrimination and cluster analysis are performed on the residual vector of fruit position error to extract abnormal change features of fruit position and posture; The abnormal change characteristics and the current position deviation data are combined to generate abnormal change characteristic data of the fruit position and posture.

7. The control method of a picking robot according to claim 6, characterized in that: S6, specifically: Based on the abnormal change characteristic data, the control parameter correction amount of the next target picking action is calculated; Update the position offset risk indicator weight in the multi-objective optimization function based on the control parameter correction amount; The updated multi-objective optimization function is used to dynamically replan the corrected robot picking path to obtain the adjusted robot picking path; Based on the control parameter correction amount and the adjusted robot picking path, the control parameters and path planning of the robot's next picking action are adjusted in real time.

Images