Dual-arm intelligent cooperative tea picking method and device based on multi-strategy dynamic scheduling

Abstract

The present application relates to a kind of based on multi-strategy dynamic scheduling's double-arm intelligent collaborative tea picking method and device, belong to robot automation field.The present application is in at, obtain the three-dimensional coordinate information of target picking point;Identify and locate potential branch and leaf shelter, generate shelter space data;By in the plane where target picking point is located, simultaneously project shelter and the jaw geometry profile under the expected grasp pose, analyze the overlap, proximity and / or density of the projection of both, generate standardized shelter evaluation index;Compare shelter evaluation index and preset decision threshold, if shelter evaluation index is greater than preset decision threshold, then execute master-slave collaborative picking method based on multi-stage positioning;Otherwise, execute parallel picking method based on dynamic collision-free region division.The present application solves the problem that traditional single-arm tea picking robot is inefficient, high collision risk, insufficient intelligent in dense, unstructured tea tree environment.

Description

Technical Field

[0001] This invention belongs to the field of robot 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 Technology

[0002] As one of the world's most important economic crops, tea harvesting has long relied heavily on manual labor, resulting in high labor costs, low efficiency, high work intensity, and significant susceptibility to weather and skill levels. With the increasing prominence of labor shortages and rising production costs, coupled with the rapid development of robotics and artificial intelligence technologies, the research and development of automated and intelligent tea-harvesting robots has become a research hotspot and development trend in modern agriculture.

[0003] Existing research on tea-picking robots mostly focuses on single-arm operation, using vision systems to identify tender buds and plan paths for harvesting. However, the tea tree growing environment is typically highly unstructured and complex: tender buds are often small and densely packed, hidden among a large number of similarly shaped and haphazardly distributed branches and leaves; branches and leaves have a certain degree of flexibility and may shift due to wind or contact with the robotic arm; the working space is narrow and riddled with obstacles. These characteristics pose significant challenges to single-arm robots, making them prone to collisions, missed harvests, misharvesting, and unreachable targets due to obstruction, severely limiting their practical application and harvesting efficiency. Summary of the Invention

[0004] In view of this, the purpose of this invention is to provide a dual-arm intelligent collaborative tea picking method and device based on multi-strategy dynamic scheduling, which solves 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 realizes intelligent, safe, efficient and automated picking operations of dual-arm tea picking device in complex tea tree canopy.

[0005] To achieve the above objectives, 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 system acquires environmental images using an RGB-D camera, identifies and segments all bud regions in the image based on the RGB images, and calculates the three-dimensional centroid of each bud mask by combining the acquired depth information or point cloud data to obtain the three-dimensional coordinate information of the target picking point. Using a morphological prior-based branch and leaf occlusion recognition algorithm, the system identifies and locates potential branch and leaf occlusions using point cloud data or RGB data combined with depth information, and generates spatial data of the occlusions.

[0007] By simultaneously projecting the occlusion and the geometric contour of the gripper in the expected grasping pose onto the plane where the target picking point is located, the overlap, proximity and / or density of the two projections are analyzed, and a standardized occlusion evaluation index is generated after normalization.

[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 region division is executed to pick the tea leaves at the target picking point.

[0009] Furthermore, the plane in which the target picking point is located is determined, providing the projection environment, including:

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

[0011] Then, define the 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 any 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 onto plane P. T Above, 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 pose X g The X-axis vector in plane P T The normalized projection vector on the plane; the direction vector of the w-axis is selected so that the direction vector of the w-axis, together with the u-axis and the normal vector of the projection plane, 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, neighboring occlusions are filtered using a preset radius R, by calculating ||p i The relationship between -r|| and R filters out the neighborhood occlusion point set B;

[0015] Then, the spatial information is projected onto the target plane, first projecting the occluding object points, for B = {b} j Point b in} j Calculate its projection onto the projection plane P. T orthogonal projection point b′ j =b j -((b j -r)·n)·n, will point bj Transform from the two-dimensional Cartesian coordinate system with the target picking point as the origin to the (u,w) coordinate system to obtain the two-dimensional obstacle projection point set B = (u′) j ,w′ j Then, for the gripper triangular mesh model M... g Each vertex v k ∈V g Applying the homogeneous transformation matrix X g V g Given a set of vertices, obtain the transformed vertex v′ in the world coordinate system. k =X g ·v k , and each vertex v′ k The orthographic projection is p′ k =v′ k -((v′ k -r)·n)·n, where n is the normal vector of the target picking point, and then p″ k "Transform into a two-dimensional coordinate system (u,w) to obtain coordinates (u″)" k ,w″ k This forms a two-dimensional vertex set V″. k Based on the two-dimensional vertex set V″ k Constructing the two-dimensional projection polygon S of the gripper g The area A is calculated using the standard polygon area formula. g =Area(S g );

[0016] Finally, interferometric analysis is performed on the two-dimensional projection information: the two-dimensional region S covered is calculated based on the two-dimensional obstacle projection point set B′. o and area A o A fine grid covering the relevant region is created on the two-dimensional plane (u,w) by rasterization, with a grid cell size of C. Each point (u′) in B′ is then traversed. j ,w′ j Mark the grid cells into which each point falls, and count the total number of uniquely marked grid cells N. Then the coverage area A is calculated. o =N×C 2 By finding those simultaneously affected by S o and S g The covered grid cells yield the obstacle projection area S. o With the gripper projection area S g The geometric intersection region S m Statistics S o and S g Calculate the overlapping area A based on the number M of grid cells that are covered by the same grid. m =M×C 2 Quantification of interference indicators The interference index K is converted into a standardized occlusion evaluation index using a Sigmoid function.

[0017] Furthermore, if the Occlusion Evaluation Index (OCI) is greater than the preset decision threshold U, then a master-slave collaborative harvesting method based on multi-stage localization is executed to harvest tea leaves, including:

[0018] First, based on the three-dimensional coordinates r of the target picking point, the spatial data O of key obstructions, and the real-time status of the two arms of the tea-picking device, the feasibility and efficiency of each arm performing picking and obstacle-clearing tasks are evaluated, and the main arm and slave arm are designated; O = {p i |p i =[x i ,y i ,z i ] T i = 1, ..., N o}, p i Let x be the 3D coordinates of the i-th key occluder. i y i , z i For its corresponding coordinate components, N o The total number of critical obstructions;

[0019] Then, the arm plans and executes the action path to remove or control the obstruction, while the main arm plans a follow path to move to a preliminary standby position near the target picking point based on the feasible space created in real time by the arm's obstacle clearing action. During the movement of the main arm, the relative position between the two arms is continuously monitored to prevent collision.

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

[0021] Furthermore, based on the three-dimensional coordinate information r of the target picking point, the spatial data O of key obstructions, and the real-time status of the tea-picking device's two arms, the main arm and the slave arm are specified, including:

[0022] Let the current poses of the ends of the two arms of the tea-picking device be P1 = [x1, y1, z1], respectively. T P2 = [x2, y2, z2] T The corresponding joint configurations are q1 and q2; calculate the kinematic reachability of each arm to r and near O, and solve the inverse motion. when If it is unattainable, it is considered reachable; if it is unattainable, a penalty term is added to the cost function. in, Let K(·) be the inverse kinematics solution for robot arm i, and A be the inverse kinematics solution function. i For the kinematic model of robotic arm i, calculate its maneuverability index and distance to the target point for any robotic arm i∈{1,2}, and construct a weighted cost function:

[0023]

[0024] In the formula, d i Let u be the Euclidean distance from robotic arm i to the target point. i The degree of maneuverability index of robotic arm i. Let C be a Jacobian matrix. i Let w be the cost value of robotic arm i. d w represents the weight of the distance to the target point. m This indicates the weight of the influence of manipulation performance; det(·) represents the determinant of the matrix; compare the cost values ​​of the two arms, and take the one with the lower cost as the main arm and the other as the slave arm;

[0025] The slave arm plans and executes the motion path for removing or controlling obstructions, while the main arm plans a follow path to move to an initial standby position near the target picking point based on the feasible space created in real time by the slave arm's obstacle removal action. This includes: using the spatiotemporal flow field method to plan the obstacle removal path for the slave arm, and constructing a spatiotemporal potential function containing attraction and repulsion fields.

[0026]

[0027] In the formula, x represents the position of the actuator at the end of the arm. As an attractor, R r Let η be the repulsion radius of the obstacle and η be the repulsion strength coefficient; by solving the differential equation Generate a continuous and adaptive obstacle-avoidance clearing path;

[0028] The main boom following planning employs nonlinear model predictive control, first establishing a simplified dynamic model of the main boom 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 for the system state x in the prediction time domain [t, t+T]. p Online optimal control problem within [the context of]:

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

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

[0031]

[0032] In the formula, a * x(τ) 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 solving the problem and is updated in a rolling manner to maintain a real-time response to the arm's movements and environmental changes.

[0033] The target bud is repositioned to obtain the final target picking point. A smooth movement trajectory is planned using a trajectory planner based on multi-stage polynomial optimization, including:

[0034] A trajectory planner based on multi-stage polynomial optimization will start from the initial standby position p s To the final picking point T f The trajectory is divided into M segments, each defined as a fifth-order polynomial. A quadratic programming problem is solved to obtain a smooth trajectory.

[0035]

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

[0037] In the formula, p m (t) represents the locus of the m-th polynomial, and a(m,j) represents the locus of the m-th fifth-order polynomial. j The coefficient of the term, T m Let M be the length of the m-th segment; the function G(·) represents the length of the segment given the number of segments M and the duration T of each segment. m Under the given conditions, minimizing the cost function yields the optimal polynomial coefficient vector; where the cost function is expressed as:

[0038]

[0039] In the formula, Indicates p m (t) 2 Perform third-order differentiation;

[0040] The main arm follows the generated smooth trajectory p(t) = Path(p m Upon reaching the target point, the gripper closes, and the gripper force sensor verifies the success of the grasp. After successful grasping, a safe evacuation trajectory is generated using the same multi-stage polynomial smooth trajectory generation method. The Path(·) function establishes the path parameters p. mThe mapping relationship between (t) and spatial position and attitude.

[0041] Among them, the initial standby position p of the main arm s The determination process includes: extracting the centroids of key occluders from the point set O. N o p is the number of points in the occluded point set. i Let p be the three-dimensional coordinates of the i-th occlusion point; define the target state p after obstacle removal. g =p c +d·l, where l is a unit vector perpendicular to the reference path rE, d is the desired displacement, and E is a matrix unit vector; based on the current pose of the main arm and the relative arrangement of r and O, calculate the initial standby position p of the main arm. s p s Conditions must be met: 1) Maintain a preset distance from the target; 2) From the current position p of the main arm i to p s The straight 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 harvesting method based on dynamic collision-free region partitioning is executed, including:

[0043] Receive the input from the parallel harvesting task and complete the basic initialization. Let the set of target points for the parallel task be r. s ={r1,r2,...,r N}, r j =[x j ,y j ,z j ] T , where r j Here, represents the 3D coordinates of the j-th target point, and N represents the number of target points. Then, the current state of each robotic arm in the tea-picking device is obtained, including the current pose of the end effectors of both arms, P1 = [x1, y1, z1]. T P2 = [x2, y2, z2] T The corresponding joints are configured as q1 and q2. Given the known spatial data O of the key occlusion objects, the entire workspace is dynamically divided into collision-free zones and tasks are assigned, as follows:

[0044] First, set the end position P of the k-th robotic arm. k As Voronoi site k , that is s k =P k , construct site collection {s k} k＝1，2This leads to the generation of a generalized Voronoi diagram in the workspace, dividing the workspace into sections. The Voronoi region of the k-th robotic arm is defined as follows:

[0045]

[0046] In the formula, v represents any point in the workspace, and s j This indicates the end position of the j-th robotic arm.

[0047] Then, by shrinking each region inward by a fixed safety distance δ, a dynamic safety region is obtained:

[0048]

[0049] In the formula, For region V k The boundary, D(·) is the function for calculating the shortest distance from point v to the boundary;

[0050] Next, the symbolic distance field D(v,P) is calculated for arm k. k Then define the region assignment function. Zone(v) represents assigning each point v to its Voronoi region V. k ;

[0051] Finally, combining the distance field assignment and safety boundary conditions, the final dynamic safe zone of the robotic arm k is:

[0052]

[0053] In obtaining the dynamic safety zone Z k After ′(t), arm k first performs trajectory planning L. k (t)=E(P k ,T k Z k P'(t)) k With T k Let E(·) be the starting point and target point of arm k, respectively; E(·) is the fifth-order polynomial trajectory function obtained by quadratic programming. After minimizing the third-order derivative energy of the trajectory through quadratic programming, a smooth fifth-order polynomial path is generated; combined with the trajectory curvature and the distance from the safety boundary, the speed is dynamically allocated so that the robotic arm can travel at high speed in a straight line and automatically decelerate when it is on a curve or close to the boundary.

[0054] After execution begins, arm k moves along the smooth trajectory L. k (t) During movement, if area contraction or potential collision risk is detected during the movement, the current trajectory is immediately interrupted and the path is retraced through the smoothing planning module E(P). k ,T k Z kGenerate a path using the '(t)' module; after reaching the picking point and completing the tea picking, use the same smooth planning module to generate a safe evacuation path; once all robotic arms have finished picking and safely evacuated, the parallel picking task ends.

[0055] Secondly, 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 surface of the chassis, a housing disposed in the central area of ​​the chassis surface, and an RGB-D camera fixed above the housing by a column. A control module is disposed within the housing, which controls the movement and picking operations of the device by executing the method described in the first aspect.

[0056] The beneficial effects of this invention are as follows:

[0057] 1) This invention constructs an intelligent multi-strategy decision-making framework, achieving a high degree of integration between perception and decision-making. This invention employs visual intelligence algorithms to directly process multimodal sensor data such as RGB-D. Through an internal joint information processing mechanism, it can simultaneously complete high-precision target localization, occlusion instance segmentation, and complex spatial relationship analysis.

[0058] 2) Based on the quantitative evaluation results output by the intelligent multi-strategy decision-making framework, this invention dynamically selects the optimal harvesting execution strategy during runtime: When the evaluation results indicate a high harvesting risk or significant spatial interference, the system activates a master-slave collaborative harvesting method based on multi-stage positioning. By clearly defining the roles of the two arms (the master arm is responsible for harvesting, and the slave arm is responsible for assisting in obstacle clearing or adjusting the environment), the slave arm performs precise obstacle clearing path planning and execution, while the master arm moves step by step to the standby and harvesting positions according to the obstacle clearing progress. Through spatiotemporal synchronization control between the master and slave arms, the success rate of harvesting and operational safety in severely obstructed environments are ensured. When the evaluation results indicate a low risk or good harvesting conditions, a parallel harvesting method based on dynamic collision-free area division is initiated. Utilizing real-time calculated and allocated dedicated safe working areas for each robotic arm, the two arms can independently and concurrently execute harvesting tasks, thereby maximizing overall harvesting efficiency.

[0059] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0060] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0061] Figure 1 This is a flowchart illustrating a dual-arm intelligent collaborative tea-picking method based on multi-strategy dynamic scheduling, provided in an embodiment of the present invention.

[0062] Figure 2 This is a schematic diagram of the structure of a dual-arm intelligent collaborative tea-picking device provided in an embodiment of the present invention. Detailed Implementation

[0063] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed 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 representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0064] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0065] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship 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 orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0066] This invention proposes a collaborative harvesting method for a dual-arm tea-picking device. The method mainly involves: acquiring environmental information through an RGB-D camera and inputting it into an intelligent harvesting decision framework. This framework uses DeepLabV3+ to identify buds and obtain harvesting points, while simultaneously using YOLOv8 to identify and segment obstructions caused by branches and leaves. Based on these identification results, an obstruction scoring module calculates the obstruction evaluation index (OCI) through projection analysis between the gripper and the obstruction. The OCI is compared with a preset threshold U: if the OCI is greater than U, it indicates severe obstruction, and a master-slave collaborative harvesting method based on multi-stage localization is selected; otherwise, a parallel harvesting method based on dynamic collision-free region partitioning is selected. The finally selected harvesting method guides the dual robotic arms to plan and execute real-time harvesting paths until the task is completed.

[0067] Based on the above, one embodiment of the present invention provides a dual-arm intelligent collaborative tea-picking method based on multi-strategy dynamic scheduling, such as... Figure 1 As shown, the method is described below:

[0068] I. An occlusion scoring module based on gripper-occlusion projection analysis is proposed. This module directly evaluates the degree of spatial interference caused by occlusions on the predetermined grasping action by using the 3D coordinates of the target point identified by DeepLabV3+ and the spatial data of occlusions segmented by YOLOv8 identified by branches and leaves. By simultaneously projecting the geometric contours of the occlusions and the gripper under the expected grasping posture onto the plane where the target picking point is located, and analyzing interference indicators such as the overlap, proximity, or density of the two projections, a standardized occlusion evaluation index is generated after normalization, providing a key basis for the selection of subsequent picking strategies (whether direct grasping or collaborative obstacle removal is required). The steps are as follows:

[0069] 1. Using the DeepLabV3+ semantic segmentation network to process the input RGB image, all bud region masks in the image are accurately identified and segmented. Then, combining the acquired depth information or point cloud data, the 3D centroid of each bud mask is calculated to obtain the 3D coordinate information r of the target picking point. Simultaneously, a morphological prior-based branch and leaf occlusion recognition algorithm is activated. Using point cloud data or RGB data combined with depth information, the geometric shape and spatial distribution characteristics of objects in the scene are analyzed, matched or classified with pre-set branch and leaf morphological prior knowledge, identifying and locating potential branch and leaf occlusions, generating occlusion spatial data, represented as a set of 3D points O = {p}. i |p i =[x i ,y i ,z i ] T i = 1, ..., N o}. p i Let x be the 3D coordinates of the i-th key occluder. i yi , z i For its corresponding coordinate components, N o This represents the total number of key obstructions.

[0070] 2. An occlusion scoring module based on gripper-occlusion projection analysis is proposed. This module quantitatively assesses the physical interference risk and generates an occlusion score by simulating the spatial occupancy conflict between the robotic arm gripper and potential occupants in the environment at the moment of final grasping. The module first receives key input information, including the 3D coordinates r of the target picking point, the 3D point set O of the identified occupants, 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. Subsequently, based on the target point r and the grasping posture X g Specific projection environment settings:

[0071] (1) Determine the normal vector of the projection plane: Usually, the grab pose 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 By setting the normal vector n of the target picking point, the equation n·(hr)=0 is satisfied, where h is any point on the plane.

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

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

[0075] (4) Calculate the Occlusion Evaluation Index (OCI):

[0076] To filter out occlusions in the neighborhood, a preset radius R is used, and ||p is calculated. i The relationship between -r|| and R filters out the neighborhood occlusion point set B.

[0077] To project spatial information onto the target plane, first project the occluding object points, for B = {b} j Point b in}j Calculate its projection onto the projection plane P. T Orthogonal projection points:

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

[0079] At the same time, point b j Transform from the two-dimensional Cartesian coordinate system with the target picking point as the origin to the (u,w) coordinate system to obtain 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 Application of grasping pose transformation X g To obtain its transformed vertex v′ in the world coordinate system k =X g ·v k Then, the transformed vertices v′ k Orthographic projection is p″ k =v′ k -((v′ k -r)·n)·n, then p″ k Transform to a two-dimensional coordinate system (u,w) to obtain coordinates (u″). k ,w″ k This forms a two-dimensional vertex set V″. k Based on this two-dimensional vertex set V″ k Constructing the two-dimensional projection polygon S of the gripper g And calculate its area A using the standard polygon area formula. g =Area(S g ).

[0081] Finally, interferometric analysis is performed on the two-dimensional projection information: First, the two-dimensional region S covered by the two-dimensional obstacle projection point set B′ is calculated. o and area A o A fine grid with a cell size of C is created on a two-dimensional plane (u,w) by rasterization, covering the relevant region. This is then used to iterate through each point (u′) in B′. j ,w′ j The grid cell into which the uniquely marked cell falls is then identified. The total number of uniquely marked grid cells, N, is counted, and the coverage area A is calculated. o =N×C 2 Then, the obstacle projection area S is calculated. o With the gripper projection area S g The geometric intersection region Sm The rasterized representation of S can be obtained by finding the simultaneous S o and S g This is achieved by covering grid cells. The overlapping area A is then calculated by counting the number M of these commonly covering grid cells. m =M×C 2 Finally, the interference index is quantified. A g If the value is greater than 0, the interference index K is converted into the standardized occlusion evaluation index OCI using the Sigmoid function.

[0082]

[0083] 2. Compare the Occlusion Evaluation Index (OCI) with the preset decision threshold U. If the OCI is greater than U, it indicates that the occlusion is severe and a collaborative obstacle removal operation is required. The master-slave collaborative harvesting method based on multi-stage positioning is executed. If the OCI is less than or equal to U, the parallel harvesting method based on dynamic collision-free region division is executed.

[0084] III. A master-slave collaborative harvesting method based on multi-stage positioning:

[0085] In the first stage, the collaborative role allocation and target determination are carried out. 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 performing the picking and obstacle clearing tasks are evaluated, and the roles of the main arm and the slave arm are clearly assigned.

[0086] In the second stage, the slave arm plans and executes the motion path to remove or control obstructions. Simultaneously, the main arm plans and executes a following path based on the feasible space created in real time by the slave arm's obstacle removal action, moving to an initial standby position near the picking point. During this process, the relative position between the two arms needs to be continuously monitored to prevent collisions. Then, when the system confirms that the obstruction has been successfully removed, it triggers the entry into the third stage.

[0087] In the third stage, after receiving the obstacle clearing 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] The steps are as follows:

[0089] 1. In the first stage, based on the 3D coordinates r of the target picking point, the spatial data O of key obstructions, and the real-time status of both arms, the master and slave roles are determined, and the operation of the left and right arms is evaluated separately. The current poses of the end effectors of both arms are P1 = [x1, y1, z1], respectively. T P2 = [x2, y2, z2] TThe corresponding joint configurations are q1 and q2, where q1 and q2 represent a set of joint angles representing the real-time posture of both arms. Based on this, the kinematic reachability of each arm to r (the tea-picking point) and approach O (the obstruction) is calculated, and an attempt is made to solve for the inverse motion. when If it is unreachable, it is considered reachable. If it is unreachable, a penalty term is added to the cost function. in, Let K(·) be the inverse kinematics solution for robot arm i, and A be the inverse kinematics solution function. i For the kinematic model of robotic arm i, calculate its maneuverability index and distance to the target point for any robotic arm i∈{1,2}, and construct a weighted cost function:

[0090]

[0091] Where, d i Let u be the Euclidean distance from robotic arm i to the target point. i The degree of maneuverability index of robotic arm i. For the corresponding Jacobian matrix, C i Let w be the cost value of robotic arm i. d w represents the weight of the distance to the target point. m This indicates the weight of the influence of manipulation performance. `det(·)` represents the determinant of the matrix. The cost values ​​of the two arms are compared; the one with the lower cost is designated as the master arm, and the other as the slave arm.

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

[0093] Finally, based on the current pose of the main arm and the relative arrangement of r and O, the intermediate standby point p of the main arm is calculated. s The point must meet the following conditions: (1) maintain a preset distance from the target; (2) from the current position p of the main arm. i to p s (3) The straight path does not collide with O; the main arm is reachable at this point, that is There is a solution.

[0094] 2: In the second stage, the slave arm plans and executes the motion path for removing or controlling obstructions. In this stage, the slave arm is responsible for the obstacle clearing action, while the master arm synchronously follows to the standby point and maintains a safe distance. First, the spatiotemporal flow field method is used to plan the obstacle clearing path for the slave arm, constructing a spatiotemporal potential function containing attractive and repulsive fields:

[0095]

[0096] Where x represents the position of the actuator at the end of the arm. It is an attractor, R r η is the repulsion radius of the obstacle, and η is the repulsion strength coefficient. This is achieved by solving the differential equation. It can generate a continuous and adaptive obstacle-avoidance clearing path.

[0097] The main boom following planning employs nonlinear model predictive control. First, a simplified dynamic model of the main boom 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, a solution is obtained for the prediction time domain [t, t+T]. p Online optimal control problem within [the context of]:

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

[0099] Where M(·) represents the initial standby position of the main arm given the current state x(t) and p. s Predicting the time domain T p Under the condition of control weight λ, the optimal control input is obtained by minimizing the cost function shown in equation (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 solving the problem and is updated in a rolling manner to maintain a real-time response to the arm's movements and environmental changes.

[0102] The two arms are synchronized via a unified clock: the slave arm initiates its movement at startup time t0, and the master arm starts its nonlinear model predictive control process after t ≥ t0 + Δt or after receiving a "clearance complete" relay signal. During the optimization process of the nonlinear model predictive control, an independent collision detection module continuously calculates the minimum distance between the complete geometric models of the two arms online. If a potential collision risk is detected, obstacle avoidance constraints between the arms are dynamically introduced or strengthened into the nonlinear model predictive control problem. When the slave arm reaches p... g The obstacle clearance is complete, and the main boom has reached a safe, initial standby position. s This phase ends when the time is up.

[0103] 3: The third stage, final positioning and precise harvesting. First, the RGB-D image information is received to reposition the target bud, obtaining the final target point T, which may have slight deviations. f =T+ΔT, and then a trajectory planner based on multi-stage polynomial optimization will be used to start from p s To T f The trajectory is divided into M segments, each segment is defined as a fifth-order polynomial, and a quadratic programming problem is solved to obtain a smooth trajectory.

[0104]

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

[0106] Where, p m (t) represents the locus of the m-th polynomial, and a(m,j) is the locus of the m-th fifth-order polynomial. j The coefficient of the term, T m Let M be the length of the m-th segment. The function G(·) represents the time interval of a given trajectory segment with a given number of segments M and a duration T of each segment. m Under the condition that, minimizing the cost function shown in equation (10) yields the optimal polynomial coefficient vector.

[0107]

[0108] in, Indicates p m (t) 2 Perform third-order differentiation.

[0109] Finally, the main arm follows the generated smooth trajectory p(t) = Path(p m Upon reaching the target point, the gripper closes, and the gripper force sensor verifies the success of the grasp. After successful grasping, a safe evacuation trajectory is generated using the same multi-stage polynomial smooth trajectory generation method. The Path(·) function establishes the path parameters p. m The mapping relationship between (t) and spatial position and attitude.

[0110] IV. Parallel Harvesting Method Based on Dynamic Collision-Free Region Division:

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

[0112] The steps are as follows:

[0113] 1: Receive the input of the parallel harvesting task and complete the basic initialization. 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 Here, represents the 3D coordinates of the j-th target point, and N is the number of target points. Then, the current state of each robotic arm is obtained, including the current pose of the end effectors P1 = [x1, y1, z1]. T P2 = [x2, y2, z2] T The corresponding joints are configured as q1 and q2. Given the known set of static occlusions O, the entire workspace is dynamically divided into collision-free regions and tasks are assigned as follows:

[0114] (1) First, perform Voronoi partitioning. Set the end position P of the k-th robotic arm... k As Voronoi site k , that is s k =P k , construct site collection {s k} k＝1，2 This process generates a generalized Voronoi diagram within the workspace, dividing the workspace accordingly. The Voronoi region of the k-th robotic arm is defined as follows:

[0115]

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

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

[0118]

[0119] in, It is region V k The boundary is defined by D(·), which is the function for calculating the shortest distance from point v to the boundary.

[0120] (3) Next, assign the range field and calculate the symbolic range field D(v,P) for arm k. k Then define the region assignment function. Where Zone(v) represents assigning each point v to its Voronoi region V. k Satisfying equation (11) for V k The definition function.

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

[0122]

[0123] 2: Obtaining the dynamic safe zone Z k After ′(t), arm k first performs trajectory planning L. k (t)=E(P k ,T k Z k ′(t)), where P k With T k Let E(·) be the starting point and target point of arm k, respectively; E(·) is the fifth-order polynomial trajectory function obtained by quadratic programming. After minimizing the third-order derivative energy of the trajectory through quadratic programming, a smooth fifth-order polynomial path is generated; then, the speed is dynamically allocated by combining the trajectory curvature and the distance from the safety boundary, so that the robotic arm can travel at high speed in a straight line and automatically decelerate when it is on a curve or close to the boundary.

[0124] After execution begins, arm k moves along the smooth trajectory L. k (t) moves and monitors its own safe zone and collision detection module in real time. Once the zone shrinks or a potential collision risk is detected, the current trajectory is immediately interrupted and the device retraces through the smoothing planning module E(P). k ,T k Z k Generate a path using the '(t)' module; after reaching the picking point and completing the tea picking, use the same smooth planning module to generate a safe evacuation path; once all robotic arms have finished picking and safely evacuated, the parallel picking task ends.

[0125] like Figure 2 As shown, another embodiment of the present invention provides a dual-arm intelligent collaborative tea-picking device, which includes an autonomous mobile platform and a dual-arm collaborative picking assembly. The autonomous mobile platform chassis adopts a tracked structure and integrates a navigation and positioning module. The dual-arm collaborative picking assembly is fixed to the autonomous mobile platform, and the assembly includes two symmetrical multi-degree-of-freedom robotic arms and customized picking grippers respectively installed at the ends of the robotic arms. An independent housing is arranged in the central area of ​​the upper surface of the chassis, and an RGB-D camera is placed on the housing support.

[0126] The autonomous mobile platform chassis adopts a robust tracked structure. The chassis has a flat rectangular shape and a track on each side. The track wraps around multiple load wheels and front and rear guide wheels, providing the tea-picking device with the ability to move in complex terrains such as tea gardens.

[0127] A separate, cubic housing is located in the central area 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 chassis's upper surface, positioned on either side of the front of the housing. Custom-designed picking grippers are attached to the ends of the robotic arms.

[0128] At the top of the box, two parallel cylindrical support columns extend vertically upwards. These two columns together support a small platform on which an RGB-D camera is mounted in the center, with the lens facing the direction of the tea-picking device's movement, providing the tea-picking device with three-dimensional visual perception of the working area in front of it.

[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 intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within 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 system acquires environmental images using an RGB-D camera, identifies and segments all bud regions in the image based on the RGB images, and calculates the three-dimensional centroid of each bud mask by combining the acquired depth information or point cloud data to obtain the three-dimensional coordinate information of the target picking point. Using a morphological prior-based branch and leaf occlusion recognition algorithm, the system identifies and locates potential branch and leaf occlusions using point cloud data or RGB data combined with depth information, and generates spatial data of the occlusions. By simultaneously projecting the occlusion and the geometric contour of the gripper in the expected grasping pose onto the plane where the target picking point is located, the overlap, proximity and / or density of the two projections are analyzed, and a standardized occlusion evaluation index is generated after normalization. Compare the occlusion evaluation index with the preset decision threshold. If the occlusion evaluation index is greater than the preset decision threshold, then execute the master-slave collaborative picking method based on multi-stage positioning 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, then execute the parallel picking method based on dynamic collision-free region division to pick the tea leaves at the target picking point. If the occlusion evaluation index is greater than the preset decision threshold, then a master-slave collaborative harvesting method based on multi-stage positioning is executed to harvest tea leaves, including: First, based on the three-dimensional coordinate information of the target picking point Key obstruction spatial data The real-time status of the two arms of the tea-picking device is monitored to assess the feasibility and efficiency of each arm in performing picking and obstacle-clearing tasks, and to designate the main arm and the slave arm. , For the first The three-dimensional coordinates of a key occluder , , Let its corresponding coordinate components be... The total number of critical obstructions; Then, the arm plans and executes the action path to remove or control the obstruction, while the main arm plans a follow path to move to a preliminary standby position near the target picking point based on the feasible space created in real time by the arm's obstacle clearing action. During the movement of the main arm, the relative position between the two arms is continuously monitored to prevent collision. Finally, once it is confirmed that the obstruction has been successfully removed, the target bud is repositioned to obtain the final target picking point. A smooth movement trajectory is planned using a trajectory planner based on multi-stage polynomial optimization. The main arm approaches the final target picking point along the smooth movement trajectory and completes the grasping action.

2. The method according to claim 1, characterized in that, Determine the plane where the target picking point is located and provide the projection environment, including: First, determine the normal vector of the projection plane and select the homogeneous transformation matrix of the expected grasping pose. of The opposite direction of the axis is taken as the normal vector. ; Then, define the projection plane. Through the normal vector of the target picking point Set, satisfying equation ,in Let be any point on the plane. r Provide the three-dimensional coordinate information of the target picking point; Finally, establish a two-dimensional coordinate system. Projection plane Above, establish a two-dimensional Cartesian coordinate system with the target picking point as the origin; select The direction vector of the axis is: the expected grasping pose. of Axial vectors in the plane The normalized projection vector on; selection The direction vector of the axis makes The direction vector of the axis and u The axes and the normal vectors of the projection plane together form a standard right-handed three-dimensional coordinate system.

3. The method according to claim 2, characterized in that, A standardized occlusion evaluation index is generated based on the plane where the target picking point is located, including: First, perform neighboring occlusion filtering using a preset radius. Through calculation and Filtering out neighboring occluded point sets based on size relationship ; Then, the spatial information is projected onto the target plane, first projecting the occluding object points, for Points in Calculate its projection onto the projection plane orthogonal projection points , will point Transform from a two-dimensional Cartesian coordinate system with the target picking point as the origin to A coordinate system is used to obtain the set of two-dimensional obstacle projection points. Then, the gripper triangular mesh model... each vertex Applying homogeneous transformation matrix , Given a set of vertices, we obtain the vertices after transformation in the world coordinate system. , to each vertex Orthographic projection is , n Let be the normal vector of the target picking point, then... Transform to two-dimensional coordinate system Coordinates obtained from This forms a two-dimensional vertex set. Based on the two-dimensional vertex set Constructing the two-dimensional projected polygon of the gripper The area is calculated using the standard polygon area formula. ; Finally, interferometric analysis is performed on the two-dimensional projection information: based on the two-dimensional obstacle projection point set. Calculate the two-dimensional region covered and area Through rasterization on a two-dimensional plane Create a fine grid that covers the relevant area, with a grid cell size of [size missing]. traversal Each point in Mark the grid cell into which each point falls, and count the total number of uniquely marked grid cells. N The coverage area By finding those who are simultaneously and The covered grid cells yield the obstacle projection area. With the gripper projection area geometric intersection region ,statistics and Number of grid cells covered by the same grid Calculate the overlapping area Quantification of interference indicators Interference indicators The occlusion evaluation index is converted to a standardized index using the Sigmoid function. .

4. The method according to claim 3, characterized in that, Based on the three-dimensional coordinate information of the target picking point Key obstruction spatial data And the real-time status of the main arm and slave arm of the tea-picking device, including: Let the current poses of the ends of the two arms of the tea-picking device be respectively. , The corresponding joint configuration is as follows , ; Calculate the arrival time of each arm and close Kinematic reachability, solving for inverse motion ,when If it is unattainable, it is considered reachable; if it is unattainable, a penalty term is added to the cost function. ;in, For robotic arms The inverse kinematic solution, To solve the function for inverse kinematics, For robotic arms Kinematic model for any robotic arm Calculate its manipulation index and distance to the target point, and construct a weighted cost function: In the formula, For robotic arms Euclidean distance to the target point For robotic arms Manipulation index For Jacobian matrices, For robotic arms Cost value, This indicates the weight of the distance to the target point. This indicates the weighting of the influence of manipulation performance; Represents the determinant of the matrix; compare the cost values ​​of the two arms, and take the one with the lower cost as the main arm and the other as the secondary arm; The slave arm plans and executes the motion path for removing or controlling obstructions, while the main arm plans a follow path to move to an initial standby position near the target picking point based on the feasible space created in real time by the slave arm's obstacle removal action. This includes: using the spatiotemporal flow field method to plan the obstacle removal path for the slave arm, and constructing a spatiotemporal potential function containing attraction and repulsion fields. In the formula, To the position of the actuator at the end of the arm, As an attraction, The repulsion radius of the obstacle. The repulsion strength coefficient is obtained by solving the differential equation. Generate a continuous and adaptive obstacle-avoidance clearing path; The main boom following planning employs nonlinear model predictive control, first establishing a simplified dynamic model of the main boom end effector. System status Including end position and speed, control input For acceleration; in each control cycle Solve a problem in the prediction time domain. The problem of online optimal control within: in, Indicates that given the current state Main arm initial standby position Prediction time domain and control weight Under the given conditions, the optimal control input is obtained by minimizing the cost function; where the cost function is expressed as: In the formula, The function for solving the optimal control sequence. To predict time variables in the time domain, for Predict the system state at time t; solve for the optimal control sequence. It performs rolling updates to keep the arm movements and environmental changes in real time; The target bud is repositioned to obtain the final target picking point. A smooth movement trajectory is planned using a trajectory planner based on multi-stage polynomial optimization, including: A trajectory planner based on multi-stage polynomial optimization will start from the initial standby position. To the final picking point The trajectory is divided into Each segment is defined as a fifth-order polynomial. A quadratic programming problem is solved to obtain a smooth trajectory. In the formula, For the first Segment polynomial locus, For the first In the fifth-order polynomial The coefficient of the term, For the first Duration of a period of time; function Indicates the number of trajectory segments and duration of each segment Under the given conditions, minimizing the cost function yields the optimal polynomial coefficient vector; where the cost function is expressed as: In the formula, Indicates to Perform third-order differentiation; The main arm follows the generated smooth trajectory Upon reaching the target point, a gripper closure command is triggered, and the gripper force sensor verifies the success of the grasp. After successful grasping, a safe evacuation trajectory is generated using the same multi-stage polynomial smooth trajectory generation method. The function establishes path parameters The mapping relationship between spatial position and attitude.

5. The method according to claim 4, characterized in that, Initial standby position of the main arm The determination process includes: from the point set Extract the centroid of key occluders , The number of points in the occlusion point set, For the first The three-dimensional coordinates of each occlusion point are defined; the target state after obstacle removal is defined. , Perpendicular to the reference path unit vector, For the desired displacement, E It is a matrix unit vector; based on the current pose of the main arm and and Based on the relative layout, calculate the initial standby position of the main arm. , Conditions must be met: 1) Maintain a preset distance from the target; 2) From the current position of the main arm arrive The straight path does not coincide with Collision; 3) There is a solution.

6. The method according to claim 3, characterized in that, If the rating index is obscured Greater than the preset decision threshold U Then, a parallel harvesting method based on dynamic collision-free region partitioning is executed, including: Receive the input from the parallel harvesting task and complete basic initialization, setting the target point set of the parallel task as follows: , ,in, It is the first The three-dimensional coordinates of the target point The number of target points is determined; then, the current state of each robotic arm of the tea-picking device is obtained, including the current pose of the end effectors of both arms. , Corresponding joint configuration , ; In the case of known key obstruction spatial data Under the premise of this, the entire workspace is dynamically divided into collision-free zones and tasks are assigned. , For the first The three-dimensional coordinates of a key occluder , , Let its corresponding coordinate components be... This represents the total number of critical obstructions; the process is as follows: First, the first The end position of the robotic arm As Voronoi site ,Right now Build a collection of sites This leads to the generation of a generalized Voronoi diagram in the workspace, which is then used to partition the workspace. The Voronoi region of a robotic arm is defined as follows: In the formula, Represents any point in the workspace. Indicates the first The end effector position of the robotic arm, Then, a fixed safety distance is fixed and moved inward from each area. This yields the dynamic safe region: In the formula, For the region The boundary For point The function for calculating the shortest distance to the boundary; Next, the arm Calculate the symbolic distance field Then define the region assignment function. , This means that each point Assigned to its Voronoi region ; Finally, combining the distance field assignment and safety boundary conditions, the robotic arm The final dynamic safe zone is: In obtaining dynamic security zone After, arm First, perform trajectory planning. , and arm The starting point and the target point; The quadratic programming method obtains a fifth-order polynomial trajectory function. The third-order derivative energy of the trajectory is minimized through quadratic programming to generate a smooth fifth-order polynomial path. The speed is then dynamically allocated by combining the trajectory curvature and the distance from the safety boundary, so that the robotic arm can travel at high speed in a straight line and automatically decelerate when it is on a curve or close to the boundary. After execution is initiated, the arm Along smooth trajectory If, during the movement, area contraction or potential collision risk is detected, the current trajectory is immediately interrupted and the path is retraced through the smoothing planning module. Generate a path; after reaching the picking point and completing the tea picking, use the same smooth planning module to generate a safe evacuation path; once all robotic arms have completed picking and safely evacuated, this parallel picking task ends.

7. A dual-arm intelligent collaborative tea-picking device, characterized in that, The device includes a chassis, tracks on both sides of the chassis, two symmetrical multi-degree-of-freedom robotic arms fixed to the surface of the chassis, a housing located in the central area of ​​the chassis surface, and an RGB-D camera fixed above the housing by a column; the housing is equipped with a control module, which controls the movement and harvesting operation of the device by executing the method of any one of claims 1 to 6.

Images