Double-arm intelligent collaborative tea picking method and device based on multi-strategy dynamic scheduling

Abstract

The invention relates to a double-arm intelligent collaborative tea picking method and device based on multi-strategy dynamic scheduling, and belongs to the field of robot automation. The method comprises the following steps: obtaining three-dimensional coordinate information of a target picking point; identifying and positioning potential branch and leaf shields, and generating space data of the shields; the method comprises the following steps: simultaneously projecting a shielding object and a clamping jaw geometric contour under an expected grabbing pose on a plane where a target picking point is located, analyzing the overlapping amount, the proximity and / or the density of the projections of the shielding object and the clamping jaw geometric contour, and generating a standardized shielding evaluation index; comparing the shielding evaluation index with a preset decision threshold, and if the shielding evaluation index is greater than the preset decision threshold, executing a master-slave collaborative picking method based on multi-stage positioning; otherwise, executing the parallel picking method based on dynamic collision-free region division. The problems that a traditional single-arm tea picking robot is low in operation efficiency, high in collision risk and insufficient in intelligent degree in a dense and unstructured tea tree environment are solved.

Description

Technical Field

[0001] The present invention belongs to the field of robotic automation technology and intelligent agriculture, and relates to a dual-arm intelligent collaborative tea picking method and device based on multi-strategy dynamic scheduling. Background Art

[0002] Tea, one of the world's most important cash crops, has long relied heavily on manual labor for its harvesting process, plagued by high labor costs, low efficiency, high workload intensity, and significant variability between weather conditions and skilled workers. With the increasing prevalence of labor shortages and rising production costs, coupled with the rapid development of robotics and artificial intelligence technologies, the development of automated, intelligent tea-picking robots has become a research hotspot and a growing trend in modern agriculture.

[0003] Existing research on tea-picking robots has mostly focused on single-arm operation, using vision systems to identify young buds and plan paths for harvesting. However, the environment in which tea trees grow is typically highly unstructured and complex: young buds are often small and dense, hidden among a large number of similar, haphazardly distributed branches and leaves; the branches and leaves are flexible and can be displaced by wind or contact with the robotic arm; and the operating space is narrow and densely populated with obstacles. These characteristics pose significant challenges for single-arm robots, making them prone to collisions, missed or mis-picked tea leaves, and inaccessible targets due to occlusion, severely limiting their practical application and harvesting efficiency. Summary of the Invention

[0004] In view of this, the purpose of the present invention is to provide a dual-arm intelligent collaborative tea picking method and device based on multi-strategy dynamic scheduling, to solve the problems of low operating efficiency, high collision risk and insufficient intelligence of traditional single-arm tea picking robots in dense and unstructured tea tree environments, and to realize the intelligent, safe and efficient automated picking operation of the dual-arm tea picking device in the complex tea tree canopy.

[0005] To achieve the above-mentioned object, the first aspect of the present invention provides a dual-arm intelligent collaborative tea picking method based on multi-strategy dynamic scheduling, the method comprising:

[0006] The RGB-D camera captures the environmental image, identifies and segments all bud area masks in the image based on the RGB image, and calculates the 3D centroid of each bud mask based on the acquired depth information or point cloud data to obtain the 3D coordinate information of the target picking point. A morphological prior-based branch and leaf occluder recognition algorithm uses point cloud data or RGB data combined with depth information to identify and locate potential branch and leaf occluders and generate spatial data of the occluders.

[0007] By simultaneously projecting the occluder and the gripper's geometric outline in the expected grasping posture onto the plane where the target picking point is located, the overlap, proximity, and / or density of the two projections are analyzed and normalized to generate a standardized occlusion evaluation index.

[0008] The occlusion evaluation index is compared with the preset decision threshold. If the occlusion evaluation index is greater than the preset decision threshold, the master-slave collaborative picking method based on multi-stage positioning is executed to pick the tea leaves at the target picking point; if the occlusion evaluation index is less than or equal to the preset decision threshold, the parallel picking method based on dynamic collision-free area division is executed to pick the tea leaves at the target picking point.

[0009] Furthermore, the plane where the target picking point is located is determined and a projection environment is provided, including:

[0010] First, determine the projection plane normal vector and select the homogeneous transformation matrix X of the expected grasping pose g The opposite direction of the Z axis is used as the normal vector,

[0011] Then, define the projection plane P T , set by the normal vector n of the target picking point, satisfying the equation n·(hr)=0, where h is an arbitrary point on the plane and r is the three-dimensional coordinate information of the target picking point;

[0012] Finally, establish a two-dimensional coordinate system (u, w) and project the plane P T On the left, establish a two-dimensional Cartesian coordinate system with the target picking point as the origin; select the direction vector of the u-axis as: the expected grasping posture X g The X-axis vector in plane P T The normalized projection vector on the projection plane is selected; the direction vector of the w-axis is selected so that the direction vector of the w-axis, the u-axis and the normal vector of the projection plane together form a standard right-handed three-dimensional coordinate system.

[0013] Furthermore, a standardized occlusion evaluation index is generated based on the plane where the target picking point is located, including:

[0014] First, perform neighborhood occlusion screening, use the preset radius R, and calculate ||p i The relationship between -r|| and R selects the neighborhood occlusion point set B;

[0015] Then, the spatial information is projected onto the target plane, and the occluded object points are projected first. For B = {b j Point b in j , calculate its projection plane P T The orthogonal projection point b′ j =b j -((b j -r)·n)·n, point bj ′ is converted from the two-dimensional Cartesian coordinate system with the target picking point as the origin to the (u, w) coordinate system, and the two-dimensional obstacle projection point set B = (u′ j ,w′ j ); then the gripper triangular mesh model M g Each vertex v k ∈V g Apply the homogeneous transformation matrix X g , V g For the vertex set, get the vertex v′ transformed in the world coordinate system k =X g ·v k , each vertex v′ k The orthogonal projection is p′ k =v′ k -((v′ k -r)·n)·n, n is the normal vector of the target picking point, and then p″ k ″Convert to the two-dimensional coordinate system (u, w) to get the coordinate (u″ k ,w″ k ), forming a two-dimensional vertex set V″ k ; According to the two-dimensional vertex set V″ k Construct the two-dimensional projection polygon S of the gripper g , and use the standard polygon area formula to calculate the area A g =Area(S g );

[0016] Finally, the interference analysis of the two-dimensional projection information is performed: the covered two-dimensional area S is calculated based on the two-dimensional obstacle projection point set B′ o and area A o , create a fine grid covering the relevant area on the two-dimensional plane (u, w) by rasterization, the grid unit size is C, and traverse each point (u′ in B′ j ,w′ j ), mark the grid cells where each point falls, count the total number of unique grid cells marked N, and then the coverage area A o =N×C 2 ; By finding out o and S g The covered grid cells are used to obtain the obstacle projection area S o and the jaw projection area S g The geometric intersection area S m , statistics S o and S g The number of grid cells M that are covered together, and the calculation of the overlapping area A m =M×C 2 ; Quantify interference indicators The interference index K is converted into a standardized occlusion evaluation index through the Sigmoid function

[0017] Furthermore, if the occlusion evaluation index OCI is greater than a preset decision threshold U, a master-slave collaborative picking method based on multi-stage positioning is executed to pick tea leaves, including:

[0018] First, based on the three-dimensional coordinate information r of the target picking point, the spatial data O of the key obstruction, and the real-time status of the two arms of the tea picking device, the feasibility and efficiency of each arm in performing the picking and obstacle removal tasks are evaluated, and the master arm and the slave arm are designated; O = {p i |p i =[x i ,y i ,z i ] T ,i=1,...,N o}, p i is the three-dimensional coordinate of the i-th key occluder, x i ,y i , z i Its corresponding coordinate component, N o is the total number of key occluders;

[0019] The slave arm then plans and executes a path to remove or control the obstruction. Simultaneously, the master arm plans a follow-up path based on the feasible space created in real time by the slave arm's obstacle-clearing action, moving to a preliminary standby position near the target picking point. During the master arm's movement, the relative position of the two arms is continuously monitored to prevent collisions.

[0020] Finally, when it is confirmed that the obstruction has been successfully removed, the target sprout is relocated to obtain the final target picking point. A trajectory planner based on multi-stage polynomial optimization is used to plan a smooth movement trajectory. The main arm approaches the final target picking point along the smooth movement trajectory and completes the grasping action.

[0021] Furthermore, the master arm and the slave arm are designated according to the three-dimensional coordinate information r of the target picking point, the spatial data O of the key obstruction, and the real-time status of the two arms of the tea picking device, including:

[0022] Assume that the current positions of the ends of the two arms of the tea picking device are P1 = [x1, y1, z1] T P2=[x2,y2,z2] T , the corresponding joint configurations are q1, q2; calculate the kinematic reachability of each arm to reach r and approach O, and solve the inverse kinematics when If it is not reachable, a penalty term is added to the cost function. in, is the inverse kinematics solution of robot arm i, K(·) is the inverse kinematics solution function, A i For the kinematic model of the robot arm i, the manipulator index and the distance to the target point are calculated for any robot arm i∈{1,2}, and a weighted cost function is constructed:

[0023]

[0024] Where, d i is the Euclidean distance from the robot arm i to the target point, u i is the manipulability index of robot arm i, is the Jacobian matrix, C i is the cost value of robot arm i, w d Indicates the distance influence weight of the target point, w m represents the weight of the control performance; det(·) represents the determinant of the matrix; the cost values ​​of the two arms are compared, and the one with lower cost is selected as the master arm, and the other is the slave arm;

[0025] The slave arm plans and executes the action path to remove or control the obstruction. At the same time, the master arm plans a follow-up path based on the feasible space created in real time by the slave arm's obstacle removal action to move to a preliminary standby position near the target picking point. This includes: using the space-time flow field method to plan the slave arm's obstacle removal path and constructing a space-time potential function containing attraction and repulsion fields:

[0026]

[0027] Where x is the position of the end effector of the slave arm, is the attraction term, R r is the repulsion radius of the obstacle, η is the repulsion strength coefficient; by solving the differential equation Generate a continuous and adaptive obstacle-avoiding clearing path;

[0028] The main arm following planning adopts nonlinear model predictive control and first establishes a simplified dynamic model of the main arm end effector The system state x includes the position and velocity of the terminal, and the control input a is the acceleration. In each control cycle t, a solution is obtained in the prediction time domain [t, t+T p ] Online optimal control problem within:

[0029] a * (τ)=M(x(t),p s ,T p ,λ)

[0030] Where M(·) represents the initial standby position p of the main arm at the given current state x(t). s , prediction time domain T pUnder the conditions of and control weight λ, the optimal control input is obtained by minimizing the cost function; where the cost function is expressed as:

[0031]

[0032] Where a * (τ) is the optimal control sequence solution function, τ is the time variable in the prediction time domain, and x(τ) is the predicted system state at time τ. The optimal control sequence u(τ) is obtained by solution and is updated in a rolling manner to maintain real-time response to the slave arm's movements and environmental changes.

[0033] Relocate the target buds to obtain the final target picking point, and use a trajectory planner based on multi-stage polynomial optimization to plan a smooth movement trajectory, including:

[0034] A trajectory planner based on multi-stage polynomial optimization is used to move the s To the final target picking point T f The trajectory is divided into M segments, each segment is defined as a fifth-order polynomial, and the quadratic programming problem is solved to obtain a smooth trajectory:

[0035]

[0036] a * (m,j)=G(M,T m )

[0037] Where p m (t) is the mth segment polynomial trajectory, a(m,j) is the t in the mth segment fifth-order polynomial j The coefficient of the term, T m is the length of the mth segment; the function G(·) represents the time duration of each segment given the number of trajectory segments M and T m Under the condition of , the optimal polynomial coefficient vector is obtained by minimizing the cost function; where the cost function is expressed as:

[0038]

[0039] Where, Indicates that p m (t) 2 Take the third-order derivative;

[0040] The main arm generates a smooth trajectory p(t)=Path(p m After reaching the target point (t), the gripper closing command is triggered, and the gripper force sensor feedback is used to verify whether the grasping is successful. After the grasping is successful, a safe evacuation trajectory is obtained using the same multi-stage polynomial smoothing trajectory generation method. Among them, the Path(·) function establishes the path parameter p m(t) The mapping relationship between the spatial position and posture.

[0041] Among them, the initial standby position of the main arm p s The determination process includes: extracting the centroid of the key occluder from the point set O N o is the number of points in the occlusion point set, p i is the three-dimensional coordinate of the i-th occlusion point; defines the target state p after the obstacle is cleared g =p c +d·l, l is the unit vector perpendicular to the reference path rE, d is the expected displacement, and E is the matrix unit vector; based on the current posture of the main arm and the relative layout of r and O, the preliminary standby position p of the main arm is calculated s , p s Conditions are met: 1) Maintain the preset distance from the target; 2) From the current position of the main arm p i to p s The straight line path does not collide with O; 3) There is a solution.

[0042] Furthermore, if the occlusion evaluation index OCI is greater than the preset decision threshold U, a parallel picking method based on dynamic collision-free area division is executed, including:

[0043] Receive the input of the parallel picking task and complete the basic initialization, and let the target point set of the parallel task be r s ={r1,r2,...,r N}, r j =[x j ,y j ,z j ] T , where r j is the three-dimensional coordinate of the jth target point, N is the number of target points; then obtain the current state of each robotic arm of the tea picking device, including the current posture of the end of the two arms P1 = [x1, y1, z1] T P2=[x2,y2,z2] T , corresponding to joint configurations q1 and q2; under the premise of knowing the key occluder spatial data O, the entire workspace is divided into dynamic collision-free areas and tasks are assigned. The process is as follows:

[0044] First, the end position P of the kth robot arm k As Voronoi sites k , that is, s k =P k , build site collections {s k} k＝1，2, and then a generalized Voronoi diagram is generated in the workspace to partition the workspace, and the Voronoi region of the kth robot is defined as:

[0045]

[0046] where v represents an arbitrary point in the workspace, s j represents the end position of the jth robot.

[0047] Then, the dynamic safety region is obtained by contracting each region inwardly by a fixed safety distance δ:

[0048]

[0049] where is the boundary of the region V k , and D(·) is a function for calculating the shortest distance from the point v to the boundary;

[0050] Next, the signed distance field D(v, P k ) is calculated for the robot k, and then the region assignment function Zone(v) is defined, which indicates that each point v is assigned to the Voronoi region V k in which it is located.

[0051] Finally, the final dynamic safety region of the robot k is obtained by combining the distance field assignment and the safety boundary condition:

[0052]

[0053] After obtaining the dynamic safety region Z k '(t), the robot k first performs trajectory planning L k (t) = E(P k , T k , Z k '(t)), where P k and T k are the start point and target point of the robot k respectively, and E(·) is a quadratic programming for obtaining a five-order polynomial trajectory function, which generates a five-order polynomial smooth path by minimizing the third-order derivative energy of the trajectory through quadratic programming; and then the velocity is dynamically assigned in combination with the trajectory curvature and the distance from the safety boundary, so that the robot automatically slows down when traveling in a straight line at high speed or approaching the boundary;

[0054] After starting execution, the robot k moves along the smooth trajectory L k (t), and if a region contraction or potential collision risk is found during the movement, the current trajectory is interrupted and the smooth planning module E(P k , T k , Z k′(t)) generates a path; after reaching the picking point and completing the tea leaves, the smooth planning module is also used to generate a safe evacuation path; when all robotic arms complete picking and safely evacuate, this parallel picking task ends.

[0055] In a second aspect, the present invention provides a dual-arm intelligent collaborative tea picking device, comprising a chassis, tracks on both sides of the chassis, two symmetrical multi-degree-of-freedom robotic arms fixed to the chassis surface, a housing disposed in the center of the chassis surface, and an RGB-D camera fixed above the housing via a column. A control module is disposed within the housing, and the control module controls the movement and picking operations of the device by executing the method described in the first aspect.

[0056] The beneficial effects of the present invention are:

[0057] 1) This invention builds an intelligent, multi-strategy decision-making framework, achieving a high degree of integration between perception and decision-making. It employs visual intelligence algorithms to directly process multimodal sensor data, including RGB-D. Through its internal joint information processing mechanism, it simultaneously achieves high-precision target positioning, instance segmentation of occluded objects, and analysis of complex spatial relationships.

[0058] 2) Based on the quantitative evaluation results output by the intelligent multi-strategy decision-making framework, the present invention dynamically selects the optimal picking execution strategy during operation: when the evaluation results indicate that the picking risk is high or there is significant spatial interference, the system activates a master-slave collaborative picking method based on multi-stage positioning. By clarifying the role allocation of the two arms (the master arm is responsible for picking, and the slave arm is responsible for assisting in obstacle clearance or adjusting the environment), the slave arm performs precise obstacle clearance path planning and execution, and the master arm moves to the standby and picking positions in steps according to the obstacle clearance progress. The spatiotemporal synchronization control between the master and slave arms is used to ensure the picking success rate and operational safety in severe occlusion environments. When the evaluation results indicate that the risk is low or the picking conditions are good, a parallel picking method based on dynamic collision-free area division is initiated. The exclusive safe operating area calculated in real time and assigned to each robotic arm is used to enable the two arms to independently and concurrently perform picking tasks, thereby maximizing the overall picking efficiency.

[0059] Other advantages, objects, and features of the present invention will be described in part in the following description and, in part, will be apparent to those skilled in the art upon examination of the following description or may be learned from practice of the present invention. The objects and other advantages of the present invention may be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below with reference to the accompanying drawings, in which:

[0061] Figure 1 A schematic diagram of a process flow of a dual-arm intelligent collaborative tea picking method based on multi-strategy dynamic scheduling provided by an embodiment of the present invention;

[0062] Figure 2 This is a schematic structural diagram of a dual-arm intelligent collaborative tea picking device provided in one embodiment of the present invention. DETAILED DESCRIPTION

[0063] The following describes the embodiments of the present invention by means of specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0064] Among them, the accompanying drawings are only for illustrative purposes and represent only schematic diagrams rather than actual pictures, and should not be understood as limiting the present invention. In order to better illustrate the embodiments of the present invention, some parts of the accompanying drawings may be omitted, enlarged or reduced, and do not represent the dimensions of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions may be omitted in the accompanying drawings.

[0065] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "front", "back", etc. indicating directions or positional relationships, they are based on the directions or positional relationships shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operate in a specific direction. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0066] The present application provides a dual-arm tea picking device cooperative picking method, which mainly comprises: obtaining environmental information through an RGB-D camera and inputting it into an intelligent picking decision framework, which uses DeepLabV3+ to identify tender buds to obtain picking points, and uses YOLOv8 to identify and segment branch and leaf obstructions. Based on these identification results, an obstruction scoring module is used to calculate the obstruction evaluation index OCI through the projection analysis of the gripper and the obstruction. The OCI is compared with the preset threshold U: if the OCI is greater than U, it indicates that the obstruction is serious, and a master-slave cooperative picking method based on multi-stage positioning is selected; otherwise, a parallel picking method based on dynamic non-collision area division is selected. The selected picking method guides the dual-arm to plan and execute the real-time picking path until the task is completed.

[0067] According to the above, an embodiment of the present application provides a dual-arm intelligent cooperative tea picking method based on multi-strategy dynamic scheduling, as shown in Figure 1 , the method is as follows:

[0068] I. An obstruction scoring module based on gripper-obstruction projection analysis is proposed. Based on the target point three-dimensional coordinates identified by DeepLabV3+ and the obstruction space data segmented by YOLOv8 branch and leaf obstruction, the module directly evaluates the degree of spatial interference caused by the obstruction to the predetermined grabbing action itself. By projecting the obstruction and the gripper geometry profile under the expected grabbing posture on the plane where the target picking point is located at the same time, and analyzing the overlap, proximity or density of the two projections, etc. interference indicators, after normalization, a standardized obstruction evaluation index is generated, which provides a key basis for the selection of subsequent picking strategies (whether to directly grab or need to cooperate with obstacle removal). The steps are as follows:

[0069] 1: Use DeepLabV3+ semantic segmentation network to process the input RGB image, accurately identify and segment all tender bud area masks in the image. Then, combined with the obtained depth information or point cloud data, calculate the three-dimensional centroid of each tender bud mask to obtain the three-dimensional coordinate information r of the target picking point. At the same time, start the branch and leaf obstruction recognition algorithm based on morphological prior, use point cloud data or combine depth information with RGB data, analyze the geometric shape and spatial distribution characteristics of objects in the scene, and match or classify them with the preset branch and leaf morphological prior knowledge, identify and locate potential branch and leaf obstructions, and generate obstruction space data, represented as a set of three-dimensional points O={p i |p i =[x i ,y i ,z i ] T ,i=1,...,N o}。p i is the three-dimensional coordinate of the ith key obstruction, x i , yi , z i Its corresponding coordinate component, N o is the total number of key occluders.

[0070] 2: An occlusion scoring module based on gripper-occluder projection analysis is proposed. This module quantitatively evaluates the physical interference risk and generates an occlusion score by simulating the spatial conflict between the manipulator gripper and potential occluders in the environment at the final grasping moment. The module first receives key input information, including the 3D coordinates of the target picking point r, the identified 3D point set of occluders O, and the gripper triangular mesh model M. g =(V g ,F g ), vertex set V g , patch set F g and the homogeneous transformation matrix of the expected grasping pose Then, according to the target point r and grasping posture X g , specifically set the projection environment:

[0071] (1) Determine the normal vector of the projection plane: Usually, the grasping posture X is selected g The opposite direction of the Z axis is used as the normal vector, that is, the third column is extracted from the rotation matrix and normalized.

[0072] (2) Define the projection plane P T : Projection plane P T The normal vector n of the target picking point is set to satisfy the equation n·(hr)=0, where h is an arbitrary point on the plane.

[0073] (3) Establish a two-dimensional plane coordinate system (u, w):

[0074] On the projection plane P containing the target picking point and with the normal vector T On the target picking point, a two-dimensional Cartesian coordinate system is established. The direction vector of the u-axis is selected and defined as: the expected grasping posture X g The X-axis vector in plane P T The normalized projection vector on the projection plane is selected; the direction vector of the w-axis is selected so that the direction vector of the w-axis, the u-axis and the normal vector of the projection plane together form a standard right-handed three-dimensional coordinate system.

[0075] (4) Calculate the occlusion evaluation index OCI:

[0076] Perform neighborhood occlusion screening, use the preset radius R, and calculate ||p i The relationship between -r|| and R is used to filter out the neighborhood occluder point set B.

[0077] Project the spatial information to the target plane, first project the occluded object points, for B = {b j Point b inj , calculate its projection plane P T The orthogonal projection point of :

[0078] b′ j =b j -((b j -r)·n)·n(1)

[0079] At the same time, point b j ′ is converted from the two-dimensional Cartesian coordinate system with the target picking point as the origin to the (u, w) coordinate system, and the two-dimensional obstacle projection point set B′=(u′ j ,w′ j ).

[0080] Next, the gripper model M g Each vertex v k ∈V g Apply grasp pose transformation X g , get the transformed vertex v′ in the world coordinate system k =X g ·v k . Then, each transformed vertex v′ k The orthogonal projection is p″ k =v′ k -((v′ k -r)·n)·n, then p″ k Convert to the two-dimensional coordinate system (u, w) to get the coordinate (u″) k ,w″ k ), forming a two-dimensional vertex set V″ k According to this two-dimensional vertex set V″ k Construct the two-dimensional projection polygon S of the gripper g , and use the standard polygon area formula to calculate its area A g =Area(S g ).

[0081] Finally, the interference analysis of the two-dimensional projection information is performed: First, the two-dimensional area S covered by the two-dimensional obstacle projection point set B′ is calculated. o and area A o By rasterization, a fine grid covering the relevant area is created on the two-dimensional plane (u, w), with a cell size of C, and each point (u′ in B′ is traversed. j ,w′ j ), mark the grid cell it falls into. Count the total number of unique grid cells marked N, and the coverage area A o =N×C 2 Then, calculate the obstacle projection area S o and the jaw projection area S g The geometric intersection area Sm The rasterized representation of this can be obtained by finding the o and S g The overlapping grid cells are realized. Count the number of these commonly covered grid cells M, and the overlapping area A m =M×C 2 , and finally quantify the interference index A g >0, the interference index K is converted into a standardized occlusion evaluation index OCI through the Sigmoid function:

[0082]

[0083] 2. Compare the occlusion evaluation index (OCI) with the preset decision threshold (U). If OCI is greater than U, it indicates that the occlusion is severe and requires collaborative obstacle removal, using a master-slave collaborative picking method based on multi-stage positioning. If OCI is less than or equal to U, a parallel picking method based on dynamic collision-free area division is implemented.

[0084] 3. Master-slave collaborative picking method based on multi-stage positioning:

[0085] In the first stage, collaborative roles are assigned and goals are determined. First, based on the three-dimensional coordinates r of the picking point, the spatial data O of key obstructions, and the real-time status of the two arms, the feasibility and efficiency of each arm in performing the picking and obstacle clearance tasks are evaluated, and the roles of the master arm and the slave arm are clearly specified.

[0086] In the second phase, the slave arm plans and executes a path to remove or control the obstruction. Simultaneously, the master arm plans and executes a follow-up path based on the feasible space created in real time by the slave arm's obstacle-clearing action, moving to a preliminary standby position near the picking point. This process requires continuous monitoring of the relative position of the two arms to prevent collisions. Once the system confirms that the obstruction has been successfully removed, it triggers the third phase.

[0087] In the third stage, after receiving the obstacle clearance completion signal, the main arm starts from the standby position, executes the final precise positioning trajectory, accurately approaches the target picking point and completes the grasping action.

[0088] Here are the steps:

[0089] 1: In the first stage, based on the 3D coordinates r of the target picking point, the spatial data O of the key occluder, and the real-time status of the two arms, the master and slave roles are determined, and the operation of the left and right arms is evaluated separately. Among them, the current poses of the two arms are P1 = [x1, y1, z1] T P2=[x2,y2,z2] T, q1, q2represent a set of joint angles of the real-time posture of the dual-arm. The kinematic accessibility of each arm to r (the tea picking point) and approaching O (the obstacle) is calculated, and the inverse kinematics is solved When , it is considered to be accessible. If it is not accessible, a penalty term is added to the cost function , where, is the inverse kinematics solution of the robot arm i, K(·) is the inverse kinematics solving function, A i is the kinematics model of the robot arm i, the manipulability index and the distance to the target point of any robot arm i∈{1, 2} are calculated, and a weighted cost function is constructed:

[0090]

[0091] , where d i is the Euclidean distance of the robot arm i to the target point, u i is the manipulability index of the robot arm i, is the corresponding Jacobian matrix, C i is the cost value of the robot arm i, w d represents the target point distance influence weight, w m represents the manipulability performance influence weight. det(·) represents the determinant of the matrix. The cost values of the two arms are compared, and the one with lower cost is taken as the master arm, and the other is taken as the slave arm.

[0092] Then the centroid of the key obstacle is extracted from the point set O , where N is the number of points in the obstacle point set, p i is the three-dimensional coordinates of the i-th obstacle point. And define the target state after clearing the obstacle p g = p c + d·l, where l is a unit vector perpendicular to the reference path r-E, and d is the desired displacement.

[0093] Finally, based on the current pose of the master arm and the relative layout of r and O, the intermediate standby point p s of the master arm is calculated, which needs to satisfy the following conditions: (1) maintaining a preset distance from the target; (2) the straight line path from the current position p i of the master arm to p s does not collide with O; (3) the master arm is accessible at this point, i.e. has a solution.

[0094] 2: In the second stage, the slave arm plans and executes the action path to remove or control the obstacle, and the master arm synchronously follows to the standby point and maintains a safe interval. First, the space-time flow field method is used for slave arm obstacle removal path planning, and a space-time potential function containing attractive and repulsive fields is constructed:

[0095]

[0096] Where x is the position of the end effector of the slave arm, is the attractor, R r is the repulsion radius of the obstacle, and η is the repulsion strength coefficient. By solving the differential equation A continuous and adaptive obstacle-avoiding clearing path can be generated.

[0097] The main arm following planning adopts nonlinear model predictive control. First, a simplified dynamic model of the main arm end effector is established. The system state x includes the position and velocity of the terminal, and the control input a is the acceleration. Then, in each control cycle t, solve a p ] Online optimal control problem within:

[0098] a * (τ)=M(x(t),p s ,T p ,λ)(6)

[0099] Where M(·) represents the initial standby position p of the main arm at the given current state x(t). s , prediction time domain T p Under the conditions of and control weight λ, the optimal control input is obtained by minimizing the cost function shown in formula (7).

[0100]

[0101] Among them, a * (τ) is the optimal control sequence solution function, τ is the time variable in the prediction time domain, and x(τ) is the predicted system state at time τ; the optimal control sequence u(τ) is obtained by solution and is rolled updated to maintain real-time response to the slave arm movement and environmental changes.

[0102] The two arms are synchronized by a unified clock: the slave arm starts to move first at the start time t0, and the master arm starts the nonlinear model predictive control process after t≥t0+Δt or after receiving the "obstacle clearance completed" relay signal. During the optimization process of nonlinear model predictive control, the minimum distance between the complete geometric models of the two arms is continuously calculated online through an independent collision detection module. If a potential collision risk is detected, the obstacle avoidance constraint between the arms is dynamically introduced or strengthened into the nonlinear model predictive control problem. When the slave arm reaches p g The obstacle clearance is completed and the main arm reaches the initial standby position p in the safe state s This stage ends when .

[0103] 3: The third stage, final positioning and accurate picking. First, accept the RGB-D image information, reposition the target sprout, and obtain the final target point T f = T + AT, then use a multi-stage polynomial optimization-based trajectory planner to divide the trajectory from p s to T f into M segments, each defined as a fifth-order polynomial, and solve a quadratic programming problem to obtain a smooth trajectory.

[0104]

[0105] a * (m, j) = G(M, T m )(9)

[0106] where p m (t) is the mth polynomial trajectory, a(m, j) is the coefficient of the t j term in the mth fifth-order polynomial, and T m is the time length of the mth segment. The function G(·) represents the optimal polynomial coefficient vector obtained by minimizing the cost function shown in equation (10) under the given trajectory segment number M and each segment duration T m .

[0107]

[0108] where, denotes the third derivative of p m (t) 2 .

[0109] Finally, the master arm reaches the target point according to the generated smooth trajectory p(t) = Path(p m (t)), and triggers the gripper closing command. The success of the grasp is verified by the feedback of the gripper force sensor; after successful grasping, a safe evacuation trajectory is obtained using the same multi-stage polynomial smoothing trajectory generation method; where the Path(·) function establishes the mapping relationship between the path parameter p m (t) and the spatial position and attitude.

[0110] Four, parallel picking method based on dynamic non-collision region division:

[0111] First, according to the current state of the dual arms and their respective picking targets, the dynamic Voronoi diagram algorithm is used to calculate and divide the exclusive non-collision working area of each arm in real time. Then, as the dual arms move, the boundaries of these safety areas are continuously updated to ensure real-time safety isolation between the arms. Finally, the updated dynamic safety areas of each arm are used as path planning constraints to drive each arm to independently generate and execute a collision-free picking path.

[0112] Here are the steps:

[0113] 1: Receive the input of the parallel picking task and complete the basic initialization, and let the target point set of the parallel task be r s ={r1,r2,...,r N}, r j =[x j ,y j ,z j ] T , where r j is the three-dimensional coordinate of the jth target point, and N is the number of target points. Then obtain the current state of each robotic arm, including the current position of the end of the arm P1 = [x1, y1, z1] T P2=[x2,y2,z2] T , the corresponding joint configurations are q1 and q2. Given the known static occluder set O, the entire workspace is divided into dynamic collision-free areas and tasks are assigned. The process is as follows:

[0114] (1) First, perform Voronoi division. The end position P of the k-th robot arm is k As Voronoi sites k , that is, s k =P k , build site collections {s k} k＝1，2 , and then generate a generalized Voronoi diagram in the workspace to divide the workspace. Among them, the Voronoi region of the k-th robot is defined as:

[0115]

[0116] Among them, v represents any point in the workspace, s j represents the end position of the j-th robotic arm.

[0117] (2) Then, the safety zone is shrunk. To avoid collisions caused by the robot arm approaching the Voronoi boundary, each zone is shrunk inward by a fixed safety distance δ to obtain a dynamic safety zone:

[0118]

[0119] in, It is region V k The boundary of , D(·) is the shortest distance calculation function from point v to the boundary.

[0120] (3) Then, assign the distance field and calculate the signed distance field D(v,P) for arm k. k ), then define the region assignment function Among them, Zone(v) means assigning each point v to the Voronoi region V where it is located k , satisfying formula (11) for V k The definition function of .

[0121] (4) Finally, combining the distance field assignment and safety boundary conditions, the final dynamic safety region of the robot k is:

[0122]

[0123] 2: Get the dynamic security area Z k ′(t), arm k first performs trajectory planning L k (t) = E(P k ,T k ,Z k ′(t)), where P k With T k where are the starting and target points of arm k, respectively. E(·) is a fifth-order polynomial trajectory function obtained through quadratic programming. Quadratic programming minimizes the third-order derivative of the trajectory to generate a fifth-order polynomial smooth path. The speed is then dynamically allocated based on the trajectory curvature and the distance to the safety boundary, allowing the robot arm to travel at high speed in a straight line and automatically decelerate when cornering or approaching the boundary.

[0124] After starting execution, arm k moves along the smooth trajectory L k (t) movement, and monitor its own safety area and collision detection module in real time. Once the area shrinks or potential collision risk is found, the current trajectory is immediately interrupted and the smooth planning module E(P k ,T k ,Z k ′(t)) generates a path; after reaching the picking point and completing the tea leaves, the smooth planning module is also used to generate a safe evacuation path; when all robotic arms complete picking and safely evacuate, this parallel picking task ends.

[0125] like Figure 2 The figure shows a dual-arm intelligent collaborative tea-picking device according to another embodiment of the present invention, comprising an autonomous mobile platform and a dual-arm collaborative picking assembly. The autonomous mobile platform chassis utilizes a crawler-type structure and integrates a navigation and positioning module. The dual-arm collaborative picking assembly is affixed to the autonomous mobile platform and comprises two symmetrical multi-degree-of-freedom robotic arms, each with a custom-designed picking gripper mounted at its end. An independent box is located in the center of the upper surface of the chassis, with an RGB-D camera mounted on the box support.

[0126] The autonomous mobile platform chassis adopts a sturdy crawler structure. The chassis has a flat rectangular shape, with a track on each side of the chassis. The tracks wrap multiple road wheels and guide wheels at the front and rear ends, providing the tea picking device with the ability to move in complex terrains such as tea gardens.

[0127] A separate cubical box sits in the center of the chassis's upper surface, housing the battery, power management system, and control module. Two identical, symmetrical, multi-degree-of-freedom robotic arms are mounted on the upper surface, one on each side of the front of the box. Customized picking grippers are attached to the arms' ends.

[0128] At the top of the box, two parallel cylindrical support columns extend vertically upward. These two columns jointly support a small platform with an RGB-D camera installed in the center of the platform. The lens faces the direction of movement of the tea picking device, providing the tea picking device with three-dimensional visual perception of the working area in front.

[0129] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should all be included in the scope of the claims of the present invention.

Claims

1. A dual-arm intelligent collaborative tea picking method based on multi-strategy dynamic scheduling, characterized in that: The method includes: The RGB-D camera captures the environmental image, identifies and segments all bud area masks in the image based on the RGB image, and calculates the 3D centroid of each bud mask based on the acquired depth information or point cloud data to obtain the 3D coordinate information of the target picking point. A morphological prior-based branch and leaf occluder recognition algorithm uses point cloud data or RGB data combined with depth information to identify and locate potential branch and leaf occluders and generate spatial data of the occluders. By simultaneously projecting the occluder and the gripper's geometric outline in the expected grasping posture onto the plane where the target picking point is located, the overlap, proximity, and / or density of the two projections are analyzed and normalized to generate a standardized occlusion evaluation index. The occlusion evaluation index is compared with the preset decision threshold. If the occlusion evaluation index is greater than the preset decision threshold, the master-slave collaborative picking method based on multi-stage positioning is executed to pick the tea leaves at the target picking point; if the occlusion evaluation index is less than or equal to the preset decision threshold, the parallel picking method based on dynamic collision-free area division is executed to pick the tea leaves at the target picking point.

2. The method according to claim 1, characterized in that Determine the plane where the target picking point is located and provide a projection environment, including: First, determine the projection plane normal vector and select the homogeneous transformation matrix X of the expected grasping pose g The opposite direction of the Z axis is used as the normal vector, Then, define the projection plane P T , set by the normal vector n of the target picking point, satisfying the equation n·(hr)=0, where h is an arbitrary point on the plane and r is the three-dimensional coordinate information of the target picking point; Finally, establish a two-dimensional coordinate system (u, w) and project the plane P T On the left, establish a two-dimensional Cartesian coordinate system with the target picking point as the origin; select the direction vector of the u-axis as: the expected grasping posture X g The X-axis vector in plane P T The normalized projection vector on the projection plane is selected; the direction vector of the w-axis is selected so that the direction vector of the w-axis, the u-axis and the normal vector of the projection plane together form a standard right-handed three-dimensional coordinate system.

3. The method according to claim 2, characterized in that Generate a standardized occlusion evaluation index based on the plane where the target picking point is located, including: First, perform neighborhood occlusion screening, use the preset radius R, and calculate ||p i The relationship between -r|| and R selects the neighborhood occlusion point set B; Then, the spatial information is projected onto the target plane, and the occluded object points are projected first. For B = {b j Point b in j , calculate its projection plane P T The orthogonal projection point b′ j =b j -((b j -r)·n)·n, point b j ′ is converted from the two-dimensional Cartesian coordinate system with the target picking point as the origin to the (u, w) coordinate system, and the two-dimensional obstacle projection point set B′=(u′ j ,w′ j ); then the gripper triangular mesh model M g Each vertex v k ∈V g Apply the homogeneous transformation matrix X g , V g For the vertex set, get the vertex v′ transformed in the world coordinate system k =X g ·v k , each vertex v′ k The orthogonal projection is p″ k =v′ k -((v′ k -r)·n)·n, n is the normal vector of the target picking point, and then p″ k Convert to the two-dimensional coordinate system (u, w) to get the coordinate (u″) k ,w″ k ), forming a two-dimensional vertex set V″ k ; According to the two-dimensional vertex set V″ k Construct the two-dimensional projection polygon S of the gripper g , and use the standard polygon area formula to calculate the area A g =Area(S g ); Finally, the interference analysis of the two-dimensional projection information is performed: the covered two-dimensional area S is calculated based on the two-dimensional obstacle projection point set B′ o and area A o , create a fine grid covering the relevant area on the two-dimensional plane (u, w) by rasterization, the grid unit size is C, and traverse each point (u′ in B′ j ,w′ j ), mark the grid cells where each point falls, count the total number of unique grid cells marked N, and then the coverage area A o =N×C 2 ; By finding out o and S g The covered grid cells are used to obtain the obstacle projection area S o Projected area S of the gripper g The geometric intersection area S m , statistics S o and S g The number of grid cells M that are covered together, and the calculation of the overlapping area A m =M×C 2 ; Quantify interference indicators The interference index K is converted into a standardized occlusion evaluation index through the Sigmoid function 4. The method according to claim 3, characterized in that If the occlusion evaluation index OCI is greater than the preset decision threshold U, the master-slave collaborative picking method based on multi-stage positioning is executed to pick tea leaves, including: First, based on the three-dimensional coordinate information r of the target picking point, the spatial data O of the key obstruction, and the real-time status of the two arms of the tea picking device, the feasibility and efficiency of each arm in performing the picking and obstacle removal tasks are evaluated, and the master arm and the slave arm are designated; O = {p i |p i =[x i ,y i ,z i ] T ,i=1,...,N o }, p i is the three-dimensional coordinate of the i-th key occluder, x i ,y i , z i Its corresponding coordinate component, N o is the total number of key occluders; The slave arm then plans and executes a path to remove or control the obstruction. Simultaneously, the master arm plans a follow-up path based on the feasible space created in real time by the slave arm's obstacle-clearing action, moving to a preliminary standby position near the target picking point. During the master arm's movement, the relative position of the two arms is continuously monitored to prevent collisions. Finally, when it is confirmed that the obstruction has been successfully removed, the target sprout is relocated to obtain the final target picking point. A trajectory planner based on multi-stage polynomial optimization is used to plan a smooth movement trajectory. The main arm approaches the final target picking point along the smooth movement trajectory and completes the grasping action.

5. The method according to claim 4, characterized in that The master arm and the slave arm are designated according to the three-dimensional coordinate information r of the target picking point, the spatial data O of the key obstruction, and the real-time status of the two arms of the tea picking device, including: Assume that the current positions of the ends of the two arms of the tea picking device are P1 = [x1, y1, z1] T P2=[x2,y2,z2] T , the corresponding joint configurations are q1, q2; calculate the kinematic reachability of each arm to reach r and approach O, and solve the inverse kinematics when If it is not reachable, a penalty term is added to the cost function. in, is the inverse kinematics solution of robot arm i, K(·) is the inverse kinematics solution function, A i For the kinematic model of the robot arm i, the manipulator index and the distance to the target point are calculated for any robot arm i∈{1,2}, and a weighted cost function is constructed: Where, d i is the Euclidean distance from the robot arm i to the target point, u i is the manipulability index of robot arm i, is the Jacobian matrix, C i is the cost value of robot arm i, w d Indicates the distance influence weight of the target point, w m represents the weight of the control performance; det(·) represents the determinant of the matrix; the cost values ​​of the two arms are compared, and the one with lower cost is selected as the master arm, and the other is the slave arm; The slave arm plans and executes the action path to remove or control the obstruction. At the same time, the master arm plans a follow-up path based on the feasible space created in real time by the slave arm's obstacle removal action to move to a preliminary standby position near the target picking point. This includes: using the space-time flow field method to plan the slave arm's obstacle removal path and constructing a space-time potential function containing attraction and repulsion fields: Where x is the position of the end effector of the slave arm, is the attraction term, R r is the repulsion radius of the obstacle, η is the repulsion strength coefficient; by solving the differential equation Generate a continuous and adaptive obstacle-avoiding clearing path; The main arm following planning adopts nonlinear model predictive control and first establishes a simplified dynamic model of the main arm end effector The system state x includes the position and velocity of the terminal, and the control input a is the acceleration. In each control cycle t, a solution is obtained in the prediction time domain [t, t+T p ] Online optimal control problem within: a * (τ)=M(x(t),p s ,T p ,l) Where M(·) represents the initial standby position p of the main arm at the given current state x(t). s , prediction time domain T p Under the conditions of and control weight λ, the optimal control input is obtained by minimizing the cost function; where the cost function is expressed as: Where a * (τ) is the optimal control sequence solution function, τ is the time variable in the prediction time domain, and x(τ) is the predicted system state at time τ. The optimal control sequence a(τ) is obtained by solution and is updated in a rolling manner to maintain real-time response to the slave arm's movements and environmental changes. Reposition the target buds to obtain the final target picking point, and use a trajectory planner based on multi-stage polynomial optimization to plan a smooth movement trajectory, including: A trajectory planner based on multi-stage polynomial optimization is used to move the s To the final target picking point T f The trajectory is divided into M segments, each segment is defined as a fifth-order polynomial, and the quadratic programming problem is solved to obtain a smooth trajectory: a * (m,j)=G(M,T m ) Where p m (t) is the mth segment polynomial trajectory, a(m,j) is the t in the mth segment fifth-order polynomial j The coefficient of the term, T m is the length of the mth segment; the function G(·) represents the time duration of each segment given the number of trajectory segments M and T m Under the condition of , the optimal polynomial coefficient vector is obtained by minimizing the cost function; where the cost function is expressed as: Where, Indicates that p m (t) 2 Take the third-order derivative; The main arm generates a smooth trajectory p(t)=Path(p m After reaching the target point (t), the gripper closing command is triggered, and the gripper force sensor feedback is used to verify whether the grasping is successful. After the grasping is successful, a safe evacuation trajectory is obtained using the same multi-stage polynomial smoothing trajectory generation method. Among them, the Path(·) function establishes the path parameter p m (t) The mapping relationship between the spatial position and posture.

6. The method according to claim 5, characterized in that The initial standby position of the main arm p s The determination process includes: extracting the centroid of the key occluder from the point set O N o is the number of points in the occlusion point set, p i is the three-dimensional coordinate of the i-th occlusion point; defines the target state p after the obstacle is cleared g =p c +d·l, l is the unit vector perpendicular to the reference path rE, d is the expected displacement, and E is the matrix unit vector; based on the current posture of the main arm and the relative layout of r and O, the preliminary standby position p of the main arm is calculated s , p s Conditions are met: 1) Maintain the preset distance from the target; 2) From the current position of the main arm p i to p s The straight line path does not collide with O; 3) There is a solution.

7. The method according to claim 3, characterized in that If the occlusion evaluation index OCI is greater than the preset decision threshold U, a parallel picking method based on dynamic collision-free area division is executed, including: Receive the input of the parallel picking task and complete the basic initialization, and let the target point set of the parallel task be r s ={r1,r2,...,r N }, r j =[x j ,y j ,z j ] T , where r j is the three-dimensional coordinate of the jth target point, N is the number of target points; then obtain the current state of each robotic arm of the tea picking device, including the current posture of the end of the two arms P1 = [x1, y1, z1] T P2=[x2,y2,z2] T , corresponding to joint configurations q1, q2; Under the premise of knowing the key occluder space data O, the entire work space is dynamically divided into collision-free areas and tasks are assigned; O = {p i |p i =[x i ,y i ,z i ] T ,i=1,...,N o }, p i is the three-dimensional coordinate of the i-th key occluder, x i ,y i , z i Its corresponding coordinate component, N o is the total number of key occluders; the process is as follows: First, the end position P of the kth robot arm is k As Voronoi sites k , that is, s k =P k , build site collections {s k } k＝1，2 , and then generate a generalized Voronoi diagram in the workspace to divide the workspace. The Voronoi region of the k-th robot is defined as: Where v represents any point in the workspace, s j represents the end position of the j-th robotic arm. Then, each area is shrunk inward by a fixed safety distance δ to obtain a dynamic safety area: Where, For region V k The boundary of , D(·) is the shortest distance calculation function from point v to the boundary; Next, the signed distance field D(v,P) is calculated for arm k. k ), then define the region assignment function Zone(v) means assigning each point v to the Voronoi region V where it is located k ; Finally, combining the distance field assignment and safety boundary conditions, the final dynamic safety region of the robot k is: In obtaining the dynamic security area Z k ′(t), arm k first performs trajectory planning L k (t) = E(P k ,T k ,Z k ′(t)), P k With T k are the starting point and target point of arm k, respectively. E(·) is a fifth-order polynomial trajectory function obtained by quadratic programming. The third-order derivative energy of the trajectory is minimized through quadratic programming to generate a fifth-order polynomial smooth path. The speed is then dynamically allocated based on the trajectory curvature and the distance to the safety boundary, so that the robot arm moves at high speed in a straight line and automatically decelerates when it is in a curve or near the boundary. After the execution is started, the arm k moves along the smooth trajectory L k (t) Movement, if the area shrinks or potential collision risk is found during the movement, the current trajectory is immediately interrupted and the smooth planning module E(P k ,T k ,Z k ′(t)) generates a path; after reaching the picking point and completing the tea leaves, the smooth planning module is also used to generate a safe evacuation path; when all robotic arms complete picking and safely evacuate, this parallel picking task ends.

8. A dual-arm intelligent collaborative tea picking device, characterized in that: The device comprises a chassis, crawlers on both sides of the chassis, two symmetrical multi-degree-of-freedom robotic arms fixed to the surface of the chassis, a box arranged in the central area of ​​the chassis surface, and an RGB-D camera fixed above the box via a column; a control module is arranged in the box, and the control module controls the movement and picking operation of the device by executing any one of the methods of claims 1 to 7.

Images