A multi-robotic arm collaborative operation control method for orchard harvesting

Abstract

This invention relates to the field of control, and more particularly to a multi-robotic arm collaborative operation control method for orchard harvesting. Based on the acquired three-dimensional coordinates of the target fruit, the original harvesting control speed, and the robotic arm's operating status data, the position of the entity sampling point at the current sampling moment is obtained. After further calculating the predicted position at the end of the next control cycle, a sweep protection domain is constructed. Based on the sweep protection domain, entity sampling point pairs are constructed, and the dynamic interval distance between entity sampling point pairs is calculated. A unit vector for the nearest distance direction is introduced to calculate the dynamic spatial compression of the entity sampling point pairs. Based on the dynamic spatial compression, the maximum dynamic spatial compression and the danger direction are obtained. The collaborative collision avoidance control speed is then calculated, and joint speed commands are generated to drive the robotic arm to perform the harvesting action. This method solves the technical problems of insufficient identification of ineffective avoidance or danger approach, robotic arm motion jitter, and harvesting trajectory deviation.

Description

Technical Field

[0001] This invention relates to the field of control, and more particularly to a multi-robotic arm collaborative operation control method for orchard harvesting. Background Technology

[0002] With the development of smart agriculture and agricultural robotics, orchard harvesting is gradually moving towards automation and intelligence. Traditional single-arm harvesting methods suffer from low harvesting efficiency, limited operating range, and insufficient continuous operation capability, making it difficult to meet the high-efficiency harvesting needs of large-scale orchards. Therefore, using multiple robotic arms to collaboratively complete fruit harvesting has become an important development direction for intelligent orchard equipment. Existing orchard harvesting robots use vision systems to identify the location of fruits and then control the robotic arms to complete the grasping and harvesting actions. However, when multiple robotic arms work simultaneously, problems such as interference between end effectors, collisions between robotic arm links, and intersections of motion trajectories can easily occur, especially in areas with dense branches and concentrated fruits, where dynamic collisions between robotic arms are more likely to occur, affecting harvesting efficiency and equipment safety. In addition, most existing multi-robotic arm control methods focus on task allocation or single-arm path planning, and do not adequately consider real-time collaborative control between multiple robotic arms. They rely solely on fixed safety distances for collision avoidance judgments, which cannot accurately reflect the dynamic approach risk of robotic arms during movement. This leads to frequent waiting, deceleration, or detours by the robotic arms, reducing the overall collaborative operation efficiency. Therefore, there is an urgent need to propose a multi-robotic arm collaborative operation control method for orchard harvesting, so as to realize the safe collaboration, dynamic collision avoidance and efficient and stable operation of multiple robotic arms in the harvesting process, improve the intelligence level of orchards, and promote the development of smart orchards and unmanned agricultural equipment. Summary of the Invention

[0003] This invention provides a multi-robotic arm collaborative operation control method for orchard harvesting, addressing the problems of existing methods that only focus on the distance between end grippers, making it difficult to cover the actual structural collisions between links, joint housings, and gripper edges; methods based on static position judgment at a single moment lack predictive analysis of the dynamic sweeping space within the robotic arm control cycle, easily leading to collision prediction lag; methods that do not fully consider the relative motion direction and relative approach speed between robotic arms, easily leading to ineffective avoidance or insufficient identification of dangerous approach; and existing methods that directly superimpose multiple risk areas, easily causing robotic arm movement jitter and harvesting trajectory deviation.

[0004] The present invention provides a multi-robotic arm collaborative operation control method for orchard harvesting, comprising the following steps: S1. Obtain the three-dimensional coordinates of the target fruit, the original picking control speed, and the operating status data of the robotic arm; The three-dimensional coordinates of the target fruit and the coordinates of each robotic arm entity sampling point in the robotic arm's operational status data are transformed to a unified orchard operation coordinate system. This yields the real-time spatial position of the target fruit in the unified orchard operation coordinate system and the position of the entity sampling point at the current sampling moment. Based on the position of the entity sampling point at the current sampling moment, the velocity of the entity sampling point is introduced to obtain the predicted position of the entity sampling point at the end of the next control cycle. Based on the position of the entity sampling point at the current sampling moment and the predicted position at the end of the next control cycle, a sweep protection domain for the robotic arm entity sampling point within the control cycle is constructed. S2. Based on the sweep protection domain, for any two different robotic arms, construct entity sampling point pairs and calculate the dynamic interval distance between entity sampling point pairs; based on the dynamic interval distance, determine the nearest spatial point pair; based on the nearest spatial point pair, obtain the nearest distance direction unit vector; based on the dynamic interval distance and the nearest distance direction unit vector, calculate the dynamic spatial compression of entity sampling point pairs; based on the dynamic spatial compression, obtain the maximum compression entity sampling point pair, the maximum dynamic spatial compression, and the danger direction; based on the maximum dynamic spatial compression and the danger direction, combined with the original picking control speed, generate a cooperative collision avoidance control speed; based on the cooperative collision avoidance control speed and the real-time spatial position of the target fruit in a unified orchard operation coordinate system, generate joint speed commands and drive the robotic arms to perform picking actions.

[0005] Preferably, S1 specifically includes: Based on the fixed control cycle of the robotic arm controller and the current sampling time, the control cycle and the end time of the next control cycle are obtained; based on the joint angular velocity in the robotic arm's operating status data, the velocity of the entity sampling point is obtained; the position of the entity sampling point at the current sampling time is added to the product of the entity sampling point's velocity and the control cycle to obtain the predicted position of the entity sampling point at the end time of the next control cycle.

[0006] Preferably, S1 specifically includes: Connect the position of the entity sampling point at the current sampling time with the predicted position at the end of the next control cycle to form a short-time motion trajectory line segment of the entity sampling point within the control cycle; based on the short-time motion trajectory line segment, combined with the circumscribed dimension of the entity structure corresponding to the entity sampling point, and introducing position uncertainty compensation, the sweep protection domain of the entity sampling point within the control cycle is obtained.

[0007] Preferably, S2 specifically includes: For each pair of entity sampling points, calculate the minimum Euclidean distance between the spatial points within the two sweep protection domains corresponding to the entity sampling point pair to obtain the dynamic interval distance between the entity sampling point pairs.

[0008] Preferably, S2 specifically includes: Based on the dynamic interval distance, the nearest spatial point pair that forms the dynamic interval distance is determined; based on the nearest spatial point pair, a direction vector is constructed, and the direction vector is normalized to obtain the nearest distance direction unit vector.

[0009] Preferably, S2 specifically includes: Based on the entity sampling point pair, the relative velocity of the two entity sampling points is obtained by subtracting their velocities; the relative velocity is then multiplied by the unit vector of the nearest distance direction to obtain the relative velocity projection of the two entity sampling points.

[0010] Preferably, S2 specifically includes: The relative approximation velocity is obtained by projecting the relative velocity of the two entity sampling points and taking the maximum value of 0. Based on the dynamic interval distance, the composite size scale of the outer radius of the corresponding entities of the two entity sampling points is introduced to calculate the distance attenuation factor. The relative approximation velocity is multiplied by the distance attenuation factor to obtain the dynamic spatial compression.

[0011] Preferably, S2 specifically includes: Based on the dynamic spatial compression quantity, the entity sampling point pair with the largest dynamic spatial compression quantity is extracted as the maximum compression entity sampling point pair; the nearest distance direction unit vector corresponding to the maximum compression entity sampling point pair is taken as the danger direction; and the dynamic spatial compression quantity corresponding to the maximum compression entity sampling point pair is taken as the maximum dynamic spatial compression quantity.

[0012] Preferably, S2 specifically includes: The product of the original harvesting control speed and the dangerous direction is multiplied by 0 and the maximum value is taken to obtain the actual dangerous approximation component in the original harvesting control speed; the dangerous direction speed component that is actually suppressed is obtained by taking the minimum value of the maximum dynamic space compression and the actual dangerous approximation component in the original harvesting control speed.

[0013] Preferably, S2 specifically includes: Based on the dangerous direction, the velocity components of the actual suppressed dangerous direction are weighted and summed, and then combined with the original picking control speed to calculate the control speed after collision avoidance correction; based on the control speed after collision avoidance correction, combined with the obtained rated maximum end effector speed of the robotic arm, a velocity saturation operator is introduced to calculate the cooperative collision avoidance control speed.

[0014] The beneficial effects of the technical solution of the present invention are: 1. By constructing physical sampling points and sweep protection domains for the robotic arm, full-process collision monitoring of the entire robotic arm structure is achieved. This avoids side collisions, joint interference, and gripper edge collisions caused by traditional methods that rely solely on end-position judgment, thus improving the safety of collaborative operations in the narrow canopy environment of orchards. 2. Based on the dynamic interval distance, relative motion direction, and relative approach speed within the control cycle, a dynamic spatial pressure quantity is constructed. This not only determines whether robotic arms are close but also identifies whether there is a continuous approaching trend, thereby reducing accidental avoidance and ineffective deceleration between robotic arms and improving the motion stability and operational continuity during multi-robotic arm collaborative harvesting. 3. By extracting the maximum pressure entity sampling point pair corresponding to the maximum dynamic spatial pressure quantity, priority suppression is applied only to the most dangerous local area. This avoids a large number of low-risk sampling points repeatedly participating in the control, improving the stability, real-time performance, and engineering feasibility of collaborative collision avoidance control, and better meeting the actual needs of prioritizing local hazards in orchard harvesting scenarios. 4. By weakening only the approach component in the dangerous direction of the original picking control speed while keeping the movement trend in the non-dangerous direction unchanged, the robotic arm can complete dynamic cooperative collision avoidance without significantly deviating from the target fruit. This balances picking efficiency and collision avoidance safety, and improves the overall work efficiency and cooperative control capability when multiple robotic arms are picking at the same time. Attached Figure Description

[0015] Figure 1 This is a flowchart of a multi-robotic arm collaborative operation control method for orchard harvesting according to the present invention. Detailed Implementation

[0016] To further illustrate the technical means and effects adopted by the present invention to achieve its intended purpose, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0017] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0018] The following description, in conjunction with the accompanying drawings, details a specific scheme for a multi-robotic arm collaborative operation control method for orchard harvesting provided by the present invention.

[0019] See attached document Figure 1 The diagram illustrates a flowchart of a multi-robotic arm collaborative operation control method for orchard harvesting according to an embodiment of the present invention. The method includes the following steps: S1. Obtain the three-dimensional coordinates of the target fruit, the original picking control speed, and the operating status data of the robotic arm; transform the coordinates of each robotic arm entity sampling point in the three-dimensional coordinates of the target fruit and the operating status data of the robotic arm to a unified orchard operation coordinate system to obtain the real-time spatial position of the target fruit in the unified orchard operation coordinate system and the position of the entity sampling point at the current sampling time; based on the position of the entity sampling point at the current sampling time, introduce the speed of the entity sampling point to obtain the predicted position of the entity sampling point at the end of the next control cycle; based on the position of the entity sampling point at the current sampling time and the predicted position at the end of the next control cycle, construct the sweep protection domain of the robotic arm entity sampling point within the control cycle.

[0020] Through the binocular vision camera mounted on the orchard picking platform or The camera acquires the 3D coordinates of the identified target fruit. These coordinates are used by the original picking trajectory planner to generate the original picking control speed for each robotic arm towards the target fruit. The robotic arm control system acquires the operational status data of each robotic arm at the current sampling moment, including joint angles, joint angular velocities, end effector pose, and robotic arm structural outline point status data. The robotic arm structural outline point status data is generated by the robotic arm control system in combination with pre-stored robotic arm structural parameters, link dimension parameters, joint shell dimension parameters, and end effector gripper dimension parameters. This data is used to characterize the real-time position of each link outline sampling point, joint shell sampling point, and end effector gripper outline sampling point in the robotic arm base coordinate system, i.e., the coordinates of each robotic arm physical sampling point.

[0021] Based on camera calibration parameters, hand-eye calibration parameters, and robotic arm base installation calibration parameters, the 3D coordinates of the target fruit, the coordinates of the robotic arm base, and the coordinates of each robotic arm entity sampling point are transformed to a unified orchard operation coordinate system. This yields the real-time spatial positions of the target fruit, robotic arm base, and each robotic arm entity sampling point within the unified orchard operation coordinate system. Camera calibration parameters are obtained using the Zhang Zhengyou calibration method after capturing images on a checkerboard or calibration board. Hand-eye calibration parameters are obtained by acquiring the camera and end effector poses from multiple robotic arm poses and solving for them. Base installation calibration parameters are obtained by measuring the installation poses of each robotic arm base in the unified orchard operation coordinate system. The coordinates of the robotic arm base and the coordinates of each robotic arm entity sampling point are derived from the state data of the robotic arm's structural outline points.

[0022] In orchard harvesting environments, collisions between multiple robotic arms do not necessarily occur between the gripper ends; more often, they may occur on the sides of the links, the elbow joint housing, or the gripper edges. Therefore, the actual structure of the robotic arm is discretized into solid sampling points to establish the first... The set of entity sampling points of a robotic arm at the current sampling moment. Each entity sampling point represents a discrete spatial position on the actual structure of the robotic arm. The entity sampling points are not limited to the end effector, but include the sampling points of the link outline, the sampling points of the joint shell, and the sampling points of the end gripper outline. This allows subsequent collision avoidance judgments to cover the overall shape of the robotic arm, rather than just focusing on the end point.

[0023] Within a control cycle, the motion of the entity sampling point can be approximated as a small displacement along the current velocity direction. The predicted position of the entity sampling point at the end of the next control cycle can be obtained by adding the product of the position of the entity sampling point at the current sampling moment, the velocity, and the control cycle.

[0024] At the end of the next control cycle The formula for calculating the predicted location of each entity sampling point is: in, Indicates the first The first robotic arm Each entity sampling point at the end of the next control cycle The predicted location, in meters; Indicates the first The first robotic arm Each entity sampling point at the current sampling time Position, number The first robotic arm The real-time spatial location of each physical sampling point in a unified orchard operation coordinate system, in meters; This indicates the fixed control cycle of the robotic arm controller, which is derived from the controller clock and is measured in seconds. Indicates the first The first robotic arm Each entity sampling point at time The speed, measured in meters per second, is obtained by mapping the joint angular velocity of the robotic arm at the current sampling moment through the forward kinematics of the robotic arm.

[0025] Based on the position of the entity sampling point at the current sampling time and the predicted position at the end of the next control cycle, the first... The first robotic arm Each entity sampling point in the control period The protected area extends outward from the entity sweep line segment within the boundary; Specifically, the first The positions of each entity sampling point at the current sampling time are connected with the predicted positions at the end of the next control cycle to form the first... The short-time motion trajectory segments of each entity sampling point within the control period; using the short-time motion trajectory segments as the spatial center trajectory, combined with the first... The external dimensions and positional uncertainty compensation of each entity sampling point correspond to the entity structure. The short-time motion trajectory segment is expanded outward along the spatial normal direction to obtain the first... The outer protection domain of the entity sweep line segment of each entity sampling point within the control period is used to describe the actual dynamic spatial occupancy range of the corresponding entity structure within the current control period. The specific calculation formula is as follows: in, Indicates the first The first robotic arm Each entity sampling point in the control period The outer protection domain formed within the solid sweep line segment, i.e., the first The first robotic arm The sweep protection zone of each physical sampling point; This represents any spatial point in the unified orchard operation coordinate system, where the coordinate unit of the spatial point is meters; This represents the set of all spatial points in a unified orchard operation coordinate system; This represents the shortest distance from a point to a line segment, expressed in meters. Indicates the first The short-time motion trajectory line segment formed by each entity sampling point from its current position to its predicted position within the current control cycle; Indicates the first The first robotic arm The external radius of the connecting rod, the external radius of the joint shell, or the external radius of the end gripper corresponding to each physical sampling point, i.e., the first... The first robotic arm The external dimensions of the entity structure corresponding to each entity sampling point are derived from the robot arm structure dimension table provided by the robot arm manufacturer, and the unit is meters. Indicates the first The position uncertainty compensation for each robotic arm is determined by linearly summing the visual depth measurement error, hand-eye calibration error, base installation calibration error, and robotic arm repeatability error. The maximum composite error is used to determine the position uncertainty compensation, with the unit being meters. The visual depth measurement error is obtained by directly reading the depth measurement accuracy parameters provided by the binocular camera or RGB-D camera manufacturer, or by directly outputting the root mean square error of depth reconstruction by calibration software such as OpenCV or MATLAB Camera Calibrator after camera calibration. The hand-eye calibration error is obtained by directly outputting the pose residual or reprojection error by the hand-eye calibration program after completing the hand-eye calibration of the robotic arm and camera, and then converting it to a three-dimensional spatial distance. The base installation calibration error is obtained by directly outputting the coordinate system registration residual by the base installation calibration program after the robotic arm base is installed, and using the residual as the base installation calibration error. The robotic arm repeatability error is obtained by directly using the repeatability accuracy parameters given in the robotic arm manufacturer's technical manual, equipment nameplate, or controller parameter file.

[0026] S2. Based on the sweep protection domain, for any two different robotic arms, construct entity sampling point pairs and calculate the dynamic interval distance between entity sampling point pairs; based on the dynamic interval distance, determine the nearest spatial point pair; based on the nearest spatial point pair, obtain the nearest distance direction unit vector; based on the dynamic interval distance and the nearest distance direction unit vector, calculate the dynamic spatial compression of entity sampling point pairs; based on the dynamic spatial compression, obtain the maximum compression entity sampling point pair, the maximum dynamic spatial compression, and the danger direction; based on the maximum dynamic spatial compression and the danger direction, combined with the original picking control speed, generate a cooperative collision avoidance control speed; based on the cooperative collision avoidance control speed and the real-time spatial position of the target fruit in a unified orchard operation coordinate system, generate joint speed commands and drive the robotic arms to perform picking actions.

[0027] For any two different robotic arms, the entity sampling points on the first robotic arm and the second robotic arm are traversed respectively to form entity sampling point pairs. For each pair, the dynamic interval distance between the two swept protection domains is calculated. The dynamic interval distance represents the shortest distance between the two entity sampling points that may occupy space within the current control cycle. Since the swept protection domains already include entity dimensions and measurement errors, the dynamic interval distance is not the ordinary Euclidean distance between two geometric center points, but rather the actual interval between the two entities' outward movement spaces. If the two swept protection domains have not yet touched, the dynamic interval distance is positive; if the two swept protection domains have touched or overlapped, the dynamic interval distance can be considered zero.

[0028] For any two different robotic arms and and arbitrary entity sampling points on the two robotic arms. and The dynamic interval distance between entity sampling point pairs within the control period is calculated using the following formula: in, Indicates the first The first robotic arm The entity sampling point and the first The first robotic arm Each entity sampling point in the current control cycle The dynamic interval distance within, in meters; Indicates the first The first robotic arm Each entity sampling point in the control period The sweeping protection zone formed within; Indicates the first The first robotic arm Each entity sampling point in the control period The sweeping protection zone formed within; Indicates sweeping protected area Points within the space; Indicates sweeping protected area Points within the space; Representing a spatial point and spatial points The Euclidean distance between them, in meters; This indicates taking the minimum distance among all spatial point combinations within the two swept protected domains.

[0029] After obtaining the dynamic interval distance, determine the nearest spatial point pair that forms the dynamic interval distance; with the first The nearest spatial point on the sweep protection zone corresponding to the robotic arm points to the first... Each robotic arm constructs a direction vector corresponding to the nearest spatial point on the swept protection domain, and performs normalization on the direction vector to obtain the nearest distance direction unit vector, which is used to characterize the most dangerous approximation direction between the swept protection domains of two entity sampling points.

[0030] Based on the dynamic interval distance, and further combined with the relative motion direction and relative approximation velocity between entity sampling points, the dynamic spatial compression of entity sampling point pairs is calculated. The dynamic spatial compression is not solely determined by distance, but considers three factors simultaneously: First, consider the relative motion direction; project the relative velocity of the two entity sampling points along the shortest distance direction to determine whether the two entity sampling points are approaching each other. If the two entity sampling points are close to each other but the motion trend is away, no dynamic spatial compression will be generated. Only when the two entity sampling points continue to approach each other along the shortest distance direction is it considered that there is a risk of collision. Second, consider the dynamic interval distance; the smaller the dynamic interval distance, the closer the two physical structures are, the higher the risk of collision, and the greater the dynamic spatial pressure. The larger the dynamic interval distance, the less dynamic spatial pressure there will be. Third, consider the size of the physical structure; for thicker connecting rods, larger joint shells, or wider gripper structures, a higher collision risk level should be required under the same dynamic interval distance; introduce a composite size scale of the outer radius of the physical entity corresponding to two physical sampling points into the dynamic spatial compression quantity to enhance the collision avoidance effect of large-sized structures.

[0031] The formula for calculating dynamic spatial compression is: in, Indicates the first The first robotic arm The entity sampling point relative to the first The first robotic arm The dynamic spatial compression formed by each physical sampling point is expressed in meters per second. Indicates the first The first robotic arm Each entity sampling point at time The speed, measured in meters per second; Indicates the first The first robotic arm Each entity sampling point at time The speed, measured in meters per second; Indicates by the first The first robotic arm The nearest spatial point of the swept protected area of ​​each entity sampling point points to the first... The first robotic arm The nearest distance direction unit vector of the swept protection domain of each entity sampling point; Indicates the first The first robotic arm The entity sampling point and the first The first robotic arm The relative velocity of each entity sampling point; Indicates the first The first robotic arm Each entity sampling point approaches the first entity along the nearest distance direction. The first robotic arm The relative velocity projection of each entity sampling point is used to represent the relative motion direction, and the unit is meters per second; This means that dynamic spatial compression occurs only when two entity sampling points approach each other along the nearest distance direction; if the two entities are far apart or remain in a non-approaching state, the dynamic spatial compression is zero. Represents the relative approximation velocity, used to indicate whether there is a relative approximation motion between two entity sampling points along the danger direction; This represents the combined size scale of the outer circumscribed radii of the two entity sampling points, in meters; Indicates the first The first robotic arm The external circumference radii of the connecting rods, the external circumference radii of the joint shells, or the external circumference radii of the end grippers corresponding to each physical sampling point are derived from the robotic arm structural dimension table provided by the robotic arm manufacturer, and the unit is meters; This represents the distance decay factor, which is the dynamic interval distance. The smaller the value, the closer it is to the decay factor. This indicates that the stronger the avoidance effect, the greater the dynamic interval distance. The larger the value, the closer it is to the decay factor. This indicates that avoiding the impact will naturally reduce it.

[0032] Furthermore, the entity sampling point pair with the largest dynamic spatial compression is extracted from all entity sampling point pairs and designated as the maximum compression entity sampling point pair. The maximum compression entity sampling point pair represents the most dangerous local entity region between the two robotic arms, the corresponding maximum dynamic spatial compression represents the collision risk intensity that most needs to be prioritized for suppression, and the corresponding closest distance direction represents the dangerous direction. By retaining only the region with the highest risk for control, a large number of distant low-risk sampling points can be avoided from repeatedly participating in avoidance, improving control stability and better meeting the actual needs of "prioritizing local dangers" in orchard picking scenarios. The specific expression is as follows: in, Indicates the first The robotic arm and the first Numbering the entity sampling points with the largest dynamic spatial compression within the current control cycle among the robotic arms; Indicates the first The sampling point number of the maximum compression entity in each robotic arm; Indicates the first The sampling point number of the largest compressed entity in each robotic arm; Indicates the first The total number of entity sampling points contained in a robotic arm; Indicates the first The total number of entity sampling points contained in a robotic arm; Indicates the first The robotic arm and the first The maximum dynamic spatial compression between robotic arms, expressed in meters per second; Indicates the first The robotic arm and the first The entity sampling point with the greatest dynamic spatial compression between the robotic arms The corresponding dynamic spatial compression; This represents the nearest distance direction unit vector corresponding to the sampling point of the entity with maximum compression, and its value range satisfies... , used to determine the first The robotic arm is relative to the first The dangerous direction of the robotic arm; Indicates the first The robotic arm and the first The entity sampling point with the greatest dynamic spatial compression between the robotic arms The corresponding nearest distance direction unit vector.

[0033] After obtaining the maximum dynamic spatial pressure and the dangerous direction, a cooperative collision avoidance control speed is generated.

[0034] Specifically, based on the acquired original picking control speed of the robotic arm, the original picking control speed is projected along the danger direction to determine whether there is a velocity component towards the danger area in the original picking action: if the velocity projection of the original picking control speed in the danger direction is less than or equal to zero, it means that the current movement of the robotic arm will not approach the danger area further, and no suppression is needed; if the velocity projection of the original picking control speed in the danger direction is greater than zero, it means that the robotic arm is indeed approaching the danger area, and only the approaching component in the danger direction is weakened without changing the movement trend in other directions, generating the first... The final collaborative collision avoidance control speed of the robotic arm is expressed as follows: in, Indicates the first A robotic arm at any time The final speed of the coordinated collision avoidance control is measured in meters per second. Indicates according to the first A velocity saturation operator that limits the rated maximum end effector speed of a robotic arm, if the modulus of the collision avoidance corrected control speed (within parentheses) does not exceed... If the magnitude of the control speed after collision avoidance correction (within parentheses) exceeds the original value, then the original value remains unchanged. Then, the direction remains unchanged and the module length is limited to... Output in the manner described; Indicates the first The rated maximum end effector speed of a robotic arm is derived from the robotic arm controller parameter table, equipment nameplate, or manufacturer's technical manual provided by the robotic arm manufacturer. The unit is meters per second, and the value range is [missing information]. ; Indicates the first The initial picking control speed of the robotic arm facing the already determined target fruit is derived from the picking trajectory planner, and the unit is meters per second; Indicates the first The robotic arm faces the already identified target fruit at the initial picking control speed in the [number]th [period]. The robotic arm points to the first The velocity projection of the robotic arm in the dangerous direction, in meters per second; This means that only the approximation component in the original picking control speed that is toward the danger direction is extracted, that is, the actual danger approximation component in the original picking control speed. If the original picking control speed is far away from the danger direction, it is zero. This indicates the actual suppressed velocity component in the dangerous direction, expressed in meters per second. The actual suppressed velocity component in the dangerous direction does not exceed the maximum dynamic space compression, nor does it exceed the actual dangerous approximation component in the original harvesting control speed. Therefore, it will not cause the robotic arm to suddenly reverse in the dangerous direction. Indicates the first The summation of the dangerous approximation components of each robotic arm relative to all other robotic arms is expressed in meters per second. Indicates the first Based on the original picking control speed of the robotic arm, and considering the collision risk with other robotic arms, only the velocity component in the direction of danger is deducted to obtain the collision avoidance corrected control speed.

[0035] The robotic arm controller will coordinate with collision avoidance control speed. The input to the inverse kinematics control module is converted into joint speed commands and used to drive the robotic arm to perform the picking action. (If collide avoidance control speed is also used...) If the direction of the original picking control speed is basically consistent, the robotic arm continues to steadily approach the target fruit. If there is a high risk of collision in a certain direction, the robotic arm only weakens the approach component in the dangerous direction, so that the robotic arm can achieve cooperative collision avoidance without significantly deviating from the picking target.

[0036] In summary, a multi-robotic arm collaborative operation control method for orchard harvesting has been developed.

[0037] The order of the embodiments is for illustrative purposes only and does not represent the superiority or inferiority of the embodiments. The processes depicted in the drawings do not necessarily require a specific or sequential order to achieve the desired result. In some embodiments, multitasking and parallel processing are possible or may be advantageous.

[0038] The various embodiments in this specification are described in a progressive manner. The same or similar parts between the various embodiments can be referred to each other. Each embodiment focuses on describing the differences from other embodiments.

[0039] 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 the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included within the protection scope of the present invention.

Claims

1. A multi-robotic arm collaborative operation control method for orchard harvesting, characterized in that, Includes the following steps: S1. Obtain the three-dimensional coordinates of the target fruit, the original picking control speed, and the operating status data of the robotic arm; The three-dimensional coordinates of the target fruit and the coordinates of each robotic arm entity sampling point in the robotic arm's operational status data are transformed to a unified orchard operation coordinate system. This yields the real-time spatial position of the target fruit in the unified orchard operation coordinate system and the position of the entity sampling point at the current sampling moment. Based on the position of the entity sampling point at the current sampling moment, the velocity of the entity sampling point is introduced to obtain the predicted position of the entity sampling point at the end of the next control cycle. Based on the position of the entity sampling point at the current sampling moment and the predicted position at the end of the next control cycle, a sweep protection domain for the robotic arm entity sampling point within the control cycle is constructed. S2. Based on the swept protection domain, for any two different robotic arms, construct a pair of entity sampling points and calculate the dynamic interval distance between the pair of entity sampling points; based on the dynamic interval distance, determine the nearest spatial point pair; Based on the nearest spatial point pair, the nearest distance direction unit vector is obtained; based on the dynamic interval distance and the nearest distance direction unit vector, the dynamic spatial compression of the entity sampling point pair is calculated; Based on the dynamic spatial compression, the maximum compression entity sampling point pair, the maximum dynamic spatial compression, and the danger direction are obtained; based on the maximum dynamic spatial compression and the danger direction, combined with the original picking control speed, a collaborative collision avoidance control speed is generated. Based on the collaborative collision avoidance control speed and the real-time spatial position of the target fruit in a unified orchard operation coordinate system, joint speed commands are generated and the robotic arm is driven to perform picking actions.

2. The multi-robotic arm collaborative operation control method for orchard harvesting according to claim 1, characterized in that, S1 specifically includes: Based on the fixed control cycle of the robotic arm controller and the current sampling time, the control cycle and the end time of the next control cycle are obtained; based on the joint angular velocity in the robotic arm's operating status data, the velocity of the entity sampling point is obtained; the position of the entity sampling point at the current sampling time is added to the product of the entity sampling point's velocity and the control cycle to obtain the predicted position of the entity sampling point at the end time of the next control cycle.

3. The multi-robotic arm collaborative operation control method for orchard harvesting according to claim 2, characterized in that, S1 specifically includes: Connect the position of the entity sampling point at the current sampling time with the predicted position at the end of the next control cycle to form a short-time motion trajectory line segment of the entity sampling point within the control cycle; based on the short-time motion trajectory line segment, combined with the circumscribed dimension of the entity structure corresponding to the entity sampling point, and introducing position uncertainty compensation, the sweep protection domain of the entity sampling point within the control cycle is obtained.

4. The multi-robotic arm collaborative operation control method for orchard harvesting according to claim 1, characterized in that, S2 specifically includes: For each pair of entity sampling points, calculate the minimum Euclidean distance between the spatial points within the two sweep protection domains corresponding to the entity sampling point pair to obtain the dynamic interval distance between the entity sampling point pairs.

5. A multi-robotic arm collaborative operation control method for orchard harvesting according to claim 4, characterized in that, S2 specifically includes: Based on the dynamic interval distance, the nearest spatial point pair that forms the dynamic interval distance is determined; based on the nearest spatial point pair, a direction vector is constructed, and the direction vector is normalized to obtain the nearest distance direction unit vector.

6. A multi-robotic arm collaborative operation control method for orchard harvesting according to claim 5, characterized in that, S2 specifically includes: Based on the entity sampling point pair, the relative velocity of the two entity sampling points is obtained by subtracting their velocities; the relative velocity is then multiplied by the unit vector of the nearest distance direction to obtain the relative velocity projection of the two entity sampling points.

7. A multi-robotic arm collaborative operation control method for orchard harvesting according to claim 6, characterized in that, S2 specifically includes: The relative approximation velocity is obtained by projecting the relative velocity of the two entity sampling points and taking the maximum value of 0. Based on the dynamic interval distance, the composite size scale of the outer radius of the corresponding entities of the two entity sampling points is introduced to calculate the distance attenuation factor. The relative approximation velocity is multiplied by the distance attenuation factor to obtain the dynamic spatial compression.

8. A multi-robotic arm collaborative operation control method for orchard harvesting according to claim 7, characterized in that, S2 specifically includes: Based on the dynamic spatial compression quantity, the entity sampling point pair with the largest dynamic spatial compression quantity is extracted as the maximum compression entity sampling point pair; the nearest distance direction unit vector corresponding to the maximum compression entity sampling point pair is taken as the danger direction; and the dynamic spatial compression quantity corresponding to the maximum compression entity sampling point pair is taken as the maximum dynamic spatial compression quantity.

9. A multi-robotic arm collaborative operation control method for orchard harvesting according to claim 8, characterized in that, S2 specifically includes: The product of the original harvesting control speed and the dangerous direction is multiplied by 0 and the maximum value is taken to obtain the actual dangerous approximation component in the original harvesting control speed; the dangerous direction speed component that is actually suppressed is obtained by taking the minimum value of the maximum dynamic space compression and the actual dangerous approximation component in the original harvesting control speed.

10. A multi-robotic arm collaborative operation control method for orchard harvesting according to claim 9, characterized in that, S2 specifically includes: Based on the dangerous direction, the velocity components of the actual suppressed dangerous direction are weighted and summed, and then combined with the original picking control speed to calculate the control speed after collision avoidance correction; based on the control speed after collision avoidance correction, combined with the obtained rated maximum end effector speed of the robotic arm, a velocity saturation operator is introduced to calculate the cooperative collision avoidance control speed.

Images