Multifunctional robot nondestructive picking and grading system based on PSO optimized hybrid polynomial trajectory

Abstract

This invention belongs to the field of robotics technology and discloses a multifunctional robotic non-destructive harvesting and grading system based on PSO-optimized hybrid polynomial trajectories. The system includes a robotic arm, an end effector, fruit boxes, and a control center. The end effector comprises a wrist unit, a support unit, a gripping and fixing unit, a stem-cutting unit, a drive system, and a pressure sensing system. During operation, the robotic arm, under the control of the control center, positions itself to the harvesting target. The gripping and fixing unit grasps the citrus fruits from bottom to top, while the pressure sensing system feeds back the sensing signals to the control center in real time to estimate the transverse diameter of the citrus and complete the coarse grading of the fruit. With the end effector firmly gripping the fruit, the stem-cutting unit performs the cutting operation. Subsequently, the control center uses a "3-5-3" polynomial interpolation algorithm to plan the motion path, driving the robotic arm to accurately place the citrus fruits into the corresponding grade's fruit box grid. This invention achieves simultaneous citrus harvesting and grading, significantly improving efficiency and saving space while eliminating the need for large secondary grading equipment, fundamentally avoiding mechanical damage caused by secondary handling.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology and relates to a harvesting robot, particularly a multifunctional robotic non-destructive harvesting and grading system for citrus fruits. Background Technology

[0002] Fruit and vegetable harvesting is highly seasonal and labor-intensive, making it difficult to complete the harvest tasks during peak farming seasons using traditional manual methods. Therefore, mechanizing and automating fruit and vegetable harvesting has become an inevitable choice to solve the labor shortage problem in agricultural production.

[0003] Fruit and vegetable harvesting robots can efficiently complete the functions of harvesting, separating fruits from stems, and collecting harvested fruits. However, the collected fruits are often mixed together, and the fruits need to be graded before being sold. Therefore, after the fruits are packed, they still need to be sorted and graded by machines. Current sorting machines occupy a large area, are costly, and can cause secondary damage to the fruits. The grading process increases the time and money costs for fruit farmers, which is not conducive to the harvesting and selling during the busy season.

[0004] Furthermore, existing harvesting robots mostly employ a single, rigid structure, making it difficult to adapt to fruits of different sizes and shapes. During the grasping process, they are highly susceptible to crushing damage to the citrus peel. Based on these problems, to maximize the economic benefits for fruit farmers, there is an urgent need to develop a new type of intelligent equipment capable of simultaneously sensing size, grasping without damage, and directly sorting and packing the fruit at the source of harvesting. Summary of the Invention

[0005] To address the technical shortcomings of existing citrus harvesting robots that increase damage, time, cost, and space costs associated with secondary mechanical grading after harvesting, this invention provides a multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory. The system employs servo motors to effectively control the end effector for stable clamping of citrus fruits, estimates radial dimensions, performs interval grading, and ensures stable harvesting and storage of citrus fruits.

[0006] The present invention achieves the above-mentioned technical objectives through the following technical means.

[0007] A multifunctional robotic non-destructive harvesting and grading system based on PSO-optimized hybrid polynomial trajectories includes an end effector, a robotic arm, and a fruit box. The end effector includes a wrist unit, a support unit, a gripping and fixing unit, a stem-cutting unit, a drive system, a pressure sensing system, and a control center. The wrist unit includes a servo motor, a turntable, and a connecting bracket. The servo motor is integrally mounted on the side of the third joint of the robotic arm and connected to the connecting bracket via the turntable. After the control unit issues a steering command, it controls the servo motor to drive the turntable to rotate, which in turn drives the connecting bracket to rotate. The connecting bracket is fixedly connected to the end effector via bolt assemblies, thereby achieving the overall torsion of the end effector, i.e., the wrist twisting motion of the robotic arm.

[0008] Furthermore, the support unit includes a vertical mounting plate and a motor mounting platform; the bottom of the vertical mounting plate is provided with bolt holes, and it is fixedly connected to the wrist unit through multiple pairs of studs; the plate surface has first to sixth mounting through holes. The motor mounting platform is welded to the vertical mounting plate.

[0009] The drive system includes a servo motor, a driving gear, a first driven gear, a second driven gear, and a first outer link, a second outer link, a first fixed link, and a second fixed link that constitute a linkage mechanism.

[0010] Furthermore, the drive gear shaft has a bore and keyway; the first and second driven gears only require bores. Half of the first and second driven gears are toothless and extend outwards with a connecting rod, the end of which has a circular through-hole. The first outer connecting rod is divided into two sections: the first section has bores at both ends for connecting rod connection, and the second section has lateral bores for pressure sensing system connection. The second outer connecting rod is divided into three sections: the first and second sections are identical to the first outer connecting rod, and the third section extends into a cylinder platform. Both the first and second fixed connecting rods have bores on both sides for connection.

[0011] Furthermore, the driving gear, the first driven gear, and the second driven gear are located on the mounting side of the vertical mounting plate; the shaft center of the driving gear is aligned with the shaft center of the first through hole, the shaft center of the first driven gear is aligned with the shaft center of the second through hole, and the shaft center of the second driven gear is aligned with the shaft center of the third through hole. The driving gear shaft has an opening and a keyway; the first and second driven gears only require openings. A sleeve is installed in the mounting hole of the vertical mounting plate, the gear shaft passes through the gear center opening and the sleeve, and the gear is mounted on the gear shaft and fixed to the elastic retaining ring by a shaft step. A deep groove ball bearing is installed on the gear shaft extending from the back side of the vertical mounting plate. The bearing is axially fixed to the sleeve by the shaft step and mounted on a flange on the back side of the vertical mounting plate, which is welded to the back side of the vertical mounting plate. The servo motor is located on the back side of the vertical mounting plate and mounted between motor mounting platforms. The servo motor shaft is connected to the driving gear shaft via a flexible coupling.

[0012] The extended connecting rods of the first driven gear and the second driven gear are all connected to the first outer connecting rod, the second outer connecting rod, the first fixed connecting rod, and the second fixed connecting rod through through holes. These through holes are connected to linear bearings using standard hinge pins, with snap rings at both ends of the pins, and the connecting rod containing the linear bearing passes through the middle. Another through hole of the first fixed connecting rod is aligned with the center axis of the fourth through hole in the vertical mounting plate, and another through hole of the second fixed connecting rod is aligned with the fifth through hole in the vertical mounting plate. Both are connected to linear bearings via standard hinge pins, and snap rings are used to secure the pins at both ends.

[0013] The pressure sensing system includes a vertical extension platform, a ball joint seat, a ball joint seat platform, and a pressure sensor. Two through holes are opened at both ends of the centerline of the vertical extension platform, aligning with two through holes on the second section of the first and second outer connecting rods, respectively, and connected by bolts and nuts. The pressure sensor is installed in the central groove of the vertical extension platform. The vertical extension platform has four columns, each equipped with a spring, and the ends of the columns are machined into studs. The ball joint seat platform has four stud holes machined at its four corners, corresponding to the four columns. The columns and stud holes should have a clearance fit of 0.1 to 0.15 mm, with axial guidance at both ends. The columns are inserted into these holes, and nuts are fitted onto the studs to limit the movement of the ball joint seat platform. The ball joint seat platform can move along the column track and returns to its original position under the action of the springs. Under the limiting effect of the fitted nuts, the ball joint seat platform will not dislodge during movement. A pressure contact rod with a spherical end is located at the bottom of the platform, which can contact and separate from the pressure sensor during the movement of the ball joint seat platform. The ball joint platform has a square ball joint seat with an internal ball chamber. The connection between the ball chamber and the outside is a flared conical surface. The pressure contact rod, the ball joint platform, and the ball joint seat are all machined as a single unit.

[0014] The clamping and fixing unit includes a ball joint, a ball joint connecting rod, a silicone mounting platform, a vertical silicone contact surface, a lower fixing platform, and a lower silicone contact surface. The ball joint has a threaded line machined inward along its central axis. One end of the ball joint connecting rod is machined as a stud, and the other end has a through hole for connection to the silicone mounting platform. The ball joint connecting rod and the ball joint are connected by a thread, and the ball joint connecting rod can rotate within a limited range along the flared conical surface of the ball joint seat. One end of the silicone mounting platform is curved, and the other end has two parallel mounting seats. Two through holes are opened on the mounting seats, which are connected to the through holes of the ball joint connecting rod via double-ended studs. Nuts are installed at both ends of the double-ended studs for fixation. The vertical silicone contact surface is adhered to the curved surface of the silicone mounting platform and fixed at the four corners with nails. The lower fixed platform has two through holes along its centerline for fixation. Four columns are located at the four corners of the platform, each with a spring installed around its circumference. The ends of the columns are machined into studs. The lower fixed platform is secured to the vertical mounting platform by two L-shaped fixing plates. Each L-shaped fixing plate has two through holes: one aligned with the center axis of a through hole on the lower fixed platform, and the other aligned with the sixth through hole on the vertical mounting platform. The through holes are secured with studs and nuts. The lower silicone disc has holes at its four corners, corresponding to the four columns of the lower fixed platform. The lower silicone disc can be fixed axially along the four columns, with nuts at its ends for limiting its position. The lower silicone disc can also move horizontally along the axis of the four columns. The lower silicone contact surface is adhered to the lower silicone disc and secured at the four corners with nails.

[0015] The fruit stem cutting unit includes a guide rod cylinder, a fixed blade, and a moving blade.

[0016] The guide rod cylinder is mounted on the cylinder platform of the third section of the second outer connecting rod and is fixed with studs and nuts; the two through holes of the fixed blade are aligned with the two through holes of the cylinder platform and are fixed with studs and nuts; the moving blade is a V-shaped blade, with a through hole at the fixed end fixed to the stud shaft of the guide rod cylinder, and both ends are fixed with nuts.

[0017] The control unit consists of a control center, pressure sensor lines, cylinder control lines, and servo motor control lines. The control center is mounted on a support unit and contains a PLC control board. The pressure sensor lines are embedded inside the connecting rod and guided to the control center, then connected to the control board via an analog-to-digital converter. The servo motor control lines are directly connected to the control board, and the cylinder control lines are embedded in the connecting rod and connected to the PLC control board inside the control center.

[0018] The non-destructive citrus harvesting steps of a multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory are as follows:

[0019] (1) The end effector moves to a position below the harvesting target according to the planned trajectory; After power-on, the end effector is in its first state. The extension connecting rods of the first and second driven gears are at a 90-degree angle to the first and second outer connecting rods. The pressure contact rod in the pressure sensing system is separated from the pressure sensor. The guide rod of the guide rod cylinder in the fruit stem shearing unit is not extended. The end effector is ready to pick the citrus fruit. After the picking robot confirms the position of the fruit to be picked, the control center calculates the motion control information required for the robotic arm's movement through trajectory planning. The robotic arm moves under the action of the control center and the joint motor, sending the end effector under the citrus fruit to be picked.

[0020] (2) The end effector clamps the citrus fruits to be harvested and performs grading calculations; The servo motor rotates forward, and through the drive system, the lower silicone contact surface of the clamping and fixing unit adaptively contacts citrus fruits of different sizes via a ball joint. The pressure contact rod contacts the pressure sensor. When the force reading of the pressure sensor reaches the preset clamping force threshold, the control center controls the servo motor to stop rotating, and the end effector firmly clamps the fruit. This is the second state of the mechanism. The servo motor feeds back the angle of rotation of this movement to the control center. The control center uses a database lookup table and interpolation method to back-calculate the transverse diameter of the citrus fruit based on the rotation angle of the servo motor, and compares it with the pre-stored transverse diameter ranges of large, medium, and small citrus fruits to determine the fruit's size and complete the online coarse grading of the citrus fruits.

[0021] (3) The end effector cuts the fruit stalk and harvests the citrus; Under the control center, the guide rod cylinder is activated, the guide rod extends, and the citrus fruit stem is hooked into the V-shaped moving blade. During the return stroke of the moving blade with the guide rod, the moving blade and the fixed blade complete the cutting of the fruit stem. This is the third state of the mechanism; the end effector has completed the harvesting of the citrus fruit.

[0022] (4) Classification and storage of harvested citrus fruits and restoration of the position of the end effector; Based on the control center's classification of the fruit and the fruit storage status of the fruit box grid stored and calculated by the control center, the control center uses trajectory planning to map the robotic arm to the required fruit box. Under the control center's action, the end effector returns to the first state and places the citrus fruit into the empty grid of the designated fruit box, completing the grading and fruit storage task. When the control center calculates that the fruit box is full, it issues an alarm reminder or automatically replaces the fruit box.

[0023] The steps of a motion trajectory planning method for a multifunctional robot-based non-destructive harvesting and grading system for citrus fruits, based on PSO-optimized hybrid polynomial trajectories, are as follows:

[0024] Step 1: Determine the relative positions of the robotic arm and the harvesting robot; Establish a motion model for the robotic arm, and use link length, link rotation angle, link offset, and joint angle to model in the DH coordinate system, and construct the coordinate transformation matrix between two adjacent links of the robotic arm; (1) In the formula, i is the link number. For rotation angle, The length of the link. Joint angle, This is the link offset. Let {i} be the homogeneous transformation matrix of coordinate system {i} relative to coordinate system {i-1}; the relative positional relationship between the manipulator and the harvesting robot is determined by the matrix transformation. Step 2: Based on the pre-obtained 3D coordinates of the citrus and the starting position of the robotic arm, the target angles of each joint of the robotic arm are solved inversely. Determine the starting coordinates of the end effector in the initial state of the robotic arm and the target coordinates of the citrus fruits to be picked. Divide the planned path into three stages, corresponding to the acceleration stage, transition stage, and deceleration stage, respectively. , , The time period is determined, and key points along the path are generated. Using the DH model established in Step 1, the Cartesian coordinates are converted into target angles for each joint through inverse kinematics. Step 3: Perform polynomial interpolation on the three trajectory segments and calculate the motion time of each segment; Two adjacent trajectory segments have the following constraints: angular displacement, angular velocity, and angular acceleration are continuous; angular velocity and angular acceleration are lower than the rated values ​​of the articulated motor. Because citrus fruits are highly susceptible to inertial swaying due to sudden acceleration changes when gripped by a flexible robotic arm and moved at high speed, leading to peel abrasions or accidental drop, a polynomial interpolation method is used to smooth the motion trajectory and acceleration. The critical path segments are connected using a "3-5-3" mixed polynomial to generate a time-varying polynomial. The changing continuous trajectory uses cubic polynomial interpolation during the acceleration and deceleration phases and fifth polynomial interpolation during the transition phase, balancing solution time and solution quality. The interpolation function is constructed using 3-5-3 polynomial interpolation as follows: (2) In the formula, Let m be the motion time of each joint of the robotic arm in segment m. For robotic arm 2 , , Interpolation function for time period, These are the unknown coefficients in the interpolation function for each segment; Step 4: Use the PSO algorithm to optimize trajectory time based on the goal of minimizing time; The coefficients of the interpolation function are used as particle vectors. To optimize the generated trajectory, the PSO algorithm is used to iterate the interpolation function based on the time minimization objective. The update formulas for particle velocity and position are as follows: (3) (4) In the formula, The position of the particle. For the velocity of the particle, Number the particles. In particle dimension, This represents the current iteration number. For inertial weights, , A random number in [0, 1] , As a learning factor, For the optimal value of an individual particle, It is the global optimum of the population; Adaptive mapping of algorithm parameters is performed based on the citrus grading results. When holding large fruits, the size is dynamically reduced. This encourages the algorithm to converge to a locally smooth trajectory earlier, reducing abrupt changes in the robotic arm's movement and ensuring smooth transport; when gripping small fruits, it increases... To enhance global optimization capabilities, shorten path planning time, and improve overall efficiency; Characterizes the local obstacle avoidance ability to circumvent fruit tree branches and trunks. It characterizes the global optimization ability to approach the target fruit box. It dynamically increases in areas with dense foliage. To enhance collision avoidance; increase size upon entering open areas. To speed up packing while ensuring both no damage and high efficiency; After the solution is completed, check the output of the solver, calculate the angular velocity and angular acceleration values ​​according to the interpolation function, and determine whether the preset constraint conditions are met. If the conditions are not met, return to solve again; if the conditions are met, output the solution results. Step 5: Solve for the motion parameters of each trajectory segment to complete the trajectory planning; After the solution is completed, the angular velocity and angular acceleration of each trajectory segment can be obtained from the angular displacement of each segment in the joint space. This allows for the continuous... The function is discretized into a series of specific angle commands according to the controller's cycle, and then sent to the joint motors to drive the robotic arm to move, thus completing the trajectory planning.

[0025] Beneficial effects of the present invention: 1. The clamping and fixing unit 14 combines a ball joint connection with a flexible silicone surface, which can adapt to citrus fruits of different sizes and shapes, effectively avoiding mechanical damage to the peel caused by rigid gripping; 2. The drive system 12 adopts a four-bar linkage, which allows for flexible design of the linkage motion trajectory. Compared with the linear clamping of the traditional worm gear, the designed envelope clamping makes the clamping more stable. The envelope clamping has lower requirements for the positioning accuracy of the end effector, eliminating the need for a precise motion sensor and reducing costs. 3. The control center 16 simultaneously completes online grading and sorting of citrus fruits by estimating their transverse diameter. This design eliminates the need for large secondary grading equipment, completely avoids damage from secondary handling of the fruit, and significantly saves time, equipment costs, and floor space. 4. The control center 16 optimizes the motion trajectory of the robotic arm 2, making the picking and storage actions quick and stable, improving work efficiency while further reducing collision damage during the movement. Attached Figure Description

[0026] Figure 1 This is a schematic diagram of the overall structure of a multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory; Figure 2 This is a schematic diagram of the wrist joint structure of a multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory. Figure 3 This is a schematic diagram of the drive system for a multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory. Figure 4 This is a schematic diagram of a pressure sensing system for a multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory. Figure 5 This is a schematic diagram of the upper clamping and fixing unit of a multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory. Figure 6 This is a schematic diagram of the lower clamping and fixing unit of a multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory. Figure 7 A schematic diagram of a fruit stem cutting unit in a multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory; Figure 8 This is a flowchart illustrating the PSO algorithm for step 4 of the trajectory planning in a multifunctional robot non-destructive harvesting and grading system based on PSO-optimized hybrid polynomial trajectories.

[0027] In the diagram, the markings are: 1-End effector, 2-Robotic arm, 3-Fruit box, 10-Wrist unit, 101-Servo motor, 102-Turntable, 103-Connecting bracket, 11-Bracket unit, 111-Motor mounting platform, 112-Vertical mounting plate, 12-Drive system, 121-Servo motor, 122-Drive gear, 123-First driven gear, 124-Second driven gear, 125-First external link, 126-Second external link, 127-First fixed link, 128-Second fixed link, 129-Cylinder platform, 13-Pressure sensing system, 131-Vertical extension platform, 132-Spherical joint platform, 133-Spherical joint, 134... - Pressure contact rod, 135- Pressure sensor, 136- Column, 14- Clamping and fixing unit, 141- Silicone mounting platform, 142- Vertical silicone contact surface, 143- Ball joint connecting rod, 144- Ball joint ball, 145- Lower silicone contact surface, 146- Lower fixing platform, 147- L-shaped fixing piece, 15- Fruit stem cutting unit, 151- Guide rod cylinder, 152- Guide rod, 153- Moving blade, 154- Fixed blade, 16- Control center, 20- Harvesting robot body, 21- First joint arm, 22- Second joint arm, 23- Third joint arm, 24- Joint motor, 31- Large fruit box, 32- Medium fruit box, 33- Small fruit box. Detailed Implementation

[0028] To more clearly demonstrate the overall collaborative operation process of the multifunctional citrus non-destructive harvesting and grading robot system of the present invention, the workflow of non-destructive citrus harvesting and grading of this system is described in detail below, in conjunction with the specific structure of each unit and the accompanying drawings. This system includes the following parts: an end effector 1, a robotic arm 2, and a fruit box 3; the end effector 1 includes a wrist unit 10, a support unit 11, a drive system 12, a pressure sensing system 13, a clamping and fixing unit 14, a fruit stem cutting unit 15, and a control center 16. The workflow of the entire system includes the following steps:

[0029] (1) Positioning and posture adjustment before picking; Before the operation begins, the control center 16 identifies the spatial coordinates of the target citrus fruit using a visual sensor. Combining this with the rotation parameters of the joint motor 24 of the robotic arm 2 and the length of the robotic arm 2, it calculates the spatial coordinates of the end effector 1, plans the path between the two coordinate points, and then drives the robotic arm 2 to move the end effector 1 under the target citrus fruit according to the planned path. Figure 1 , Figure 2 As shown, the wrist unit 10 works as follows: the control center 16 drives the servo motor 101 to rotate, which in turn drives the turntable 102 and the connecting bracket 103 to rotate, thereby realizing the overall rotation of the end effector 1, that is, realizing the twisting action of the robotic wrist, thereby adjusting the gripping and shearing mechanism to the optimal picking posture.

[0030] (2) Non-destructive envelope clamping and online coarse grading; After the end effector 1 enters the working area and adjusts to the optimal posture, the system begins the clamping and grading of the citrus fruits. For example... Figure 3 As shown, the drive system 12 operates as follows: the control center 16 controls the servo motor 121 to rotate forward, driving the drive gear 122 to rotate; the meshing second driven gear 124 rotates accordingly, driving the first driven gear 123 to rotate; the first driven gear 123 and the second driven gear 124 drive the four-bar linkage, and the first outer link 125 and the second outer link 126 of the four-bar linkage move towards each other to perform the clamping operation. By adjusting the length of the four-bar linkage members, the movement trajectory of the vertical silicone contact surface 142 can be adjusted, so that the clamping and fixing unit 14 approaches from the bottom of the citrus and then completes the enveloping clamping upward. Because of the use of enveloping clamping, the end effector 1 only needs to be positioned in a certain space at the bottom of the citrus, and the positioning accuracy requirement is not high, eliminating the need for a precise motion sensor, reducing costs while improving the clamping success rate.

[0031] During the above clamping and closing process, combined with Figure 4 , Figure 5 and Figure 6 As shown, the clamping and fixing unit 14 and the pressure sensing system 13 work together: the surface of the citrus fruit first contacts the vertical silicone contact surface 142, and the ball joint 144 rotates within a limited range in the ball joint platform 132, which can drive the vertical silicone contact surface 142 to fit citrus fruits of different sizes; the lower silicone contact surface 145 contacts the bottom of the citrus fruit, playing an auxiliary fixing role. The ball joint setting makes the clamping working surface fit the surface of the citrus fruit, minimizing the mechanical damage to the citrus fruit caused by the clamping and fixing unit 14. At the same time, as the second outer connecting rod 126 rotates, it drives the vertical extension platform 131 and the ball joint platform 132 to move closer to the center of the citrus fruit, and the pressure contact rod 134 squeezes the pressure sensor 135. When the pressure of the pressure sensor 135 reaches the set non-destructive clamping threshold, the control center 16 controls the servo motor 121 to stop rotating.

[0032] At this time, the control center 16 performs online coarse grading: the servo motor 121 feeds back the angle of the rotation of the control center 16 during this movement. The control center 16 uses the database lookup table and interpolation method to back-calculate the transverse diameter of the citrus fruit based on the rotation angle of the servo motor 121, and compares it with the pre-stored transverse diameter ranges of large, medium and small citrus fruits to determine the size of the fruit and complete the online coarse grading of the citrus fruit.

[0033] The following supplements the method for setting the clamping force threshold used by the control center 16 during picking and grading, as well as the calibration method for the grading interval of citrus, the transverse diameter of citrus, and the rotation angle of the servo motor 121.

[0034] In this embodiment, the non-destructive clamping force threshold of citrus fruits needs to be determined experimentally in advance and set in the control center 16. The determination method involves using a texture analyzer to compress a group of citrus fruits at different pressure gradients, such as 10N, 15N, and 20N. After pressure treatment, the citrus fruits are left to stand for one week. After one week, they are cut open for inspection. If mold appears, it indicates that the pressure treatment caused tearing of the internal pulp cells of the citrus fruits, resulting in damage. Based on the experimental determination, the maximum non-destructive pressure threshold of the citrus fruits is obtained, and the non-destructive clamping threshold of the control center 16 is set to 90% of this value.

[0035] In this embodiment, the citrus grading intervals need to be determined experimentally. The transverse diameter of several citrus fruits is measured to obtain the distribution of the fruit's transverse diameter, and a certain proportion is set to classify the fruit size. For example, the largest 30% are defined as large fruits, the smallest 30% as small fruits, and the middle 40% as medium fruits. The smallest transverse diameter among the large fruits is used as a threshold; fruits above this threshold are considered large fruits. Similarly, the largest transverse diameter among the small fruits is used as a threshold; fruits below this threshold are considered small fruits, and fruits between the two thresholds are considered medium fruits. This completes the division of the citrus grading intervals.

[0036] In this embodiment, the database for the transverse diameter of the citrus fruit derived by the control center 16 through the rotation angle of the servo motor 121 needs to be obtained through calibration experiments. The calibration process is as follows: the clamping and fixing unit 14 is opened to its limit position, and the initial position of the servo motor shaft is recorded at this time. A sphere with a standard diameter of 'a' and an elasticity similar to that of a citrus fruit is clamped until the pressure sensor 135 reaches the threshold and stops clamping. The ending position of the servo motor shaft is recorded at this time, and the angle between the two positions is θ. For example, when clamping a sphere with a diameter of 8cm, the rotation angle is 1000°; when clamping a sphere with a diameter of 9cm, the rotation angle is 1200°. The calibration of spheres with different diameters is performed in sequence, and a database is established. When the control center 16 derives the transverse diameter of the citrus fruit, it only needs to reverse the database. If a rotation angle of 1200° is detected, it corresponds to a citrus fruit with a transverse diameter of 9cm. If the detected rotation angle is not in the database, the interpolation method is used for calculation, and the formula is as follows: (5) In the formula, The transverse diameter of the citrus fruit to be measured. This refers to the angle between the servo motor shaft and the end position during the fruit clamping process. It is less than in the database The most recent value, It is greater than in the database The most recent value, It is in the database The horizontal diameter of the citrus fruit corresponding to the corner. It is in the database The horizontal diameter of the citrus fruit corresponding to the corner.

[0037] It should be noted that the diameter span between large, medium, and small fruits in the citrus harvesting and grading scenario targeted by this invention is typically greater than 10mm. When the clamping and fixing unit 14 clamps the fruit to the preset clamping force threshold, although the flexible silicone and ball joint structure will produce some elastic deformation, this deformation is considered a minor error in engineering. Since this deformation error is much smaller than the range of fruit grades, it has minimal impact on the final classification results of large, medium, and small fruits, and is within a completely acceptable tolerance range for engineering applications. Therefore, this invention ignores it. This approach, while ensuring undamaged clamping of the citrus, eliminates the need for complex flexible deformation compensation calculations and improves the response speed of online grading.

[0038] (3) Stable shearing of the fruit stalk; After ensuring that the end effector 1 has firmly and undamagedly gripped the citrus fruit, the shearing stage begins. For example... Figure 7 As shown, the stem-cutting unit 15 operates as follows: the control center 16 controls the solenoid valve to intake air, the guide rod 152 of the guide rod cylinder 151 extends, driving the moving blade 153 to extend synchronously. The stem enters the V-shaped working area of ​​the moving blade 153 under the guidance of the outer inclined surface of the moving blade 153; then, the guide rod 152 of the guide rod cylinder 151 retracts, and the stem breaks under the cutting of the moving blade 153 and the fixed blade 154, completing the stem-cutting task. The V-shaped shape of the moving blade 153 of this invention has strong adaptability, and even if the angle of the stem deviates to a certain extent, it can be guided back to the cutting working area.

[0039] (4) Citrus delivery and end effector reset; After the cutting is completed, the control center 16, in conjunction with the citrus fruit grading information obtained in step (2) above, confirms the corresponding fruit boxes to be placed in the large, medium, and small fruit boxes, and extracts the coordinates of the target grid of the corresponding fruit box. The fruit box 3 of the present invention is designed in the form of a three-row, four-column grid, and each grid is numbered, that is, from grid number 1 to grid number 12. Citrus fruits are placed from the smallest to the largest grid number. When the placement begins, the control center 16 obtains the following information: the type of fruit box to be placed (large, medium, small), which grid of this fruit box type, and the spatial coordinates of this grid. Based on the previously obtained spatial coordinates of the citrus and the spatial coordinates of the grid to be placed, the control center 16 performs trajectory planning between the two points, and controls the robotic arm 2 to send the end effector 1 to the top of the grid. Then, the control center 16 controls the servo motor 121 to reverse, so that the clamping and fixing unit 14 is reset. The clamping and fixing unit 14 of the end effector 1 releases the citrus and puts it into the target grid of the fruit box, thus completing the picking, grading, and packing of the target citrus fruits.

[0040] Afterwards, the control center 16 records the current position coordinates of the robotic arm 2, combines them with the coordinates of the next target citrus fruit to plan the trajectory, and begins the next round of picking, grading, and packing. As the picking progresses, when all the grids of a fruit box are filled with citrus fruits, the control center 16 issues an alarm to remind the accompanying personnel to unload and replace the fruit box, or the externally mounted automatic box-changing equipment will replace it.

[0041] The following is an embodiment of a trajectory planning method based on PSO optimized hybrid polynomial trajectory used in the citrus harvesting process according to the present invention.

[0042] Step 1: Determine the relative positions of the robotic arm and the harvesting robot; Establish a motion model for robotic arm 2, and use link length, link rotation angle, link offset, and joint angle to model in the DH coordinate system, and construct the coordinate transformation matrix between two adjacent links of robotic arm 2; (1) In the formula, i is the link number. For rotation angle, The length of the link. Joint angle, This is the link offset. Let {i} be the homogeneous transformation matrix of coordinate system {i} relative to coordinate system {i-1}; the relative positional relationship between the manipulator and the harvesting robot is determined by the matrix transformation. Step 2: Based on the pre-obtained 3D coordinates of the citrus and the starting position of the robotic arm, the target angles of each joint of the robotic arm 2 are solved inversely. The starting coordinates of the end effector 1 in the initial state of the robotic arm 2 and the target coordinates of the citrus fruits to be picked are determined. The planned picking path is divided into three stages, corresponding to the acceleration stage, the transition stage, and the deceleration stage, respectively. , , The time period is determined, and key points along the path are generated. Using the DH model established in Step 1, the Cartesian coordinates are converted into target angles for each joint through inverse kinematics. Step 3: Perform polynomial interpolation on the three trajectories and calculate the required motion time for each segment; The following constraints apply to two adjacent trajectory segments: angular displacement, angular velocity, and angular acceleration are continuous; angular velocity and angular acceleration are lower than the rated values ​​corresponding to joint motor 24. Connect the critical path segment points using a "3-5-3" mixed polynomial to generate a time-varying polynomial. The changing continuous trajectory uses cubic polynomial interpolation during the acceleration and deceleration phases and fifth polynomial interpolation during the transition phase, balancing solution time and solution quality. The interpolation function is constructed using 3-5-3 polynomial interpolation as follows: (2) In the formula, Let m be the motion time of each joint of robotic arm 2 in segment m. For robotic arm 2 , , Interpolation function for time period, These are the unknown coefficients in the interpolation function for each segment; Step 4: Use the PSO algorithm to optimize trajectory time based on the goal of minimizing time; like Figure 8 As shown, the coefficients of the interpolation function are used as particle vectors. To optimize the generated trajectory, the PSO algorithm is used to iterate the interpolation function based on the time minimization objective. The update formulas for particle velocity and position are as follows: (3) (4) In the formula, The position of the particle. For the velocity of the particle, Number the particles. In particle dimension, This represents the current iteration number. For inertial weights, , A random number in [0, 1] , As a learning factor, For the optimal value of an individual particle, It is the global optimum of the population; The algorithm parameters are adaptively optimized based on the citrus grading results. When holding large fruits (with high physical inertia), the force is dynamically reduced. This encourages the algorithm to converge to a locally smooth trajectory earlier, reducing abrupt changes in the robotic arm's movement and ensuring smooth transport; when gripping small fruits, it increases... To enhance global optimization capabilities, shorten path planning time, and improve overall efficiency; Characterizes the local obstacle avoidance ability to circumvent fruit tree branches and trunks. It characterizes the global optimization ability to approach the target fruit box. It dynamically increases in areas with dense foliage. To enhance collision avoidance; increase size upon entering open areas. To speed up packing while ensuring both no damage and high efficiency; After the solution is completed, check the output of the solver, calculate the angular velocity and angular acceleration values ​​according to the interpolation function, and determine whether the preset constraint conditions are met. If the conditions are not met, return to solve again; if the conditions are met, output the solution results. Step 5: Solve for the motion parameters of each trajectory segment to complete the trajectory planning; After the solution is completed, the angular velocity and angular acceleration of each trajectory segment can be obtained from the angular displacement of each segment in the joint space. This allows for the continuous... The function is discretized into a series of specific angle commands according to the controller's cycle, and then sent to the joint motors to drive the robotic arm to move, thus completing the trajectory planning.

[0043] The embodiments described above are preferred embodiments of the present invention, but the present invention is not limited to the above embodiments. Any obvious improvements, substitutions or modifications that can be made by those skilled in the art without departing from the essence of the present invention shall fall within the protection scope of the present invention.

Claims

1. A multifunctional robot-based non-destructive harvesting and grading system based on PSO-optimized hybrid polynomial trajectories, characterized in that, It includes an end effector (1), a robotic arm (2), a fruit box (3), and a control center (16); The end effector (1) includes a wrist unit (10), a support unit (11), a drive system (12), a pressure sensing system (13), a clamping and fixing unit (14), a stem cutting unit (15), and a control center (16); one end of the wrist unit (10) is mounted on the robotic arm (2), and the other end is fixedly connected to the support unit (11), and the other end of the support unit (11) is fixedly connected to the drive system (12); The drive system (12) is connected to the pressure sensing system (13) and the clamping and fixing unit (14); The control center (16) is configured to control the stem cutting unit (15) to cut the citrus stem based on the signal collected by the pressure sensing system (13); The robotic arm (2) includes three articulated arms and articulated motors (24). One end of the first articulated arm (21) is fixedly mounted on the harvesting robot body (20). The other end of the first articulated arm (21) is connected to one end of the second articulated arm (22) through the articulated motor (24). The other end of the second articulated arm (22) is connected to one end of the third articulated arm (23) through the articulated motor (24). The other end of the third articulated arm (23) is fixedly connected to the bracket (103) by bolts. The bracket (103) is connected to the vertical mounting plate (112) of the end effector (1) by screws and nuts. The robotic arm (2) is configured to be driven by the articulated motors (24). The wrist unit (10) includes a servo motor (101), a turntable (102), and a connecting bracket (103); The end effector (1) is mounted on the side of the third joint arm (23) via a servo motor (101). The turntable (102) is fixedly connected to the connecting bracket (103). The connecting bracket (103) is connected to the vertical mounting plate (112) of the end effector (1) via studs and nuts. The control center (16) is configured to drive the turntable (102) via the servo motor (101) and drive the connecting bracket (103) to rotate, so as to realize the overall rotation of the end effector (1), that is, to realize the twisting action of the robotic wrist. The control center (16) is connected to the drive system (12), the pressure sensing system (13) and the fruit stem cutting unit (15). The control center (16) is equipped with a trajectory planning and execution module, which is used to determine the picking motion path of the robotic arm through trajectory planning, and drive the robotic arm (2) to drive the end effector (1) to put the picked and graded citrus into the corresponding fruit box (3) to complete the non-destructive picking, grading and storage of citrus.

2. The multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory according to claim 1, characterized in that, The support unit (11) of the end effector (1) includes a motor mounting platform (111) and a vertical mounting plate (112); the vertical mounting plate (112) and the connecting bracket (103) are fixedly connected by studs and nuts; the motor mounting platform (111) is welded to the vertical mounting plate (112).

3. The multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory according to claim 1, characterized in that, The drive system (12) of the end effector (1) includes a servo motor (121), a drive gear (122), a first driven gear (123), a second driven gear (124), a first external connecting rod (125), a second external connecting rod (126), a first fixed connecting rod (127), a second fixed connecting rod (128), a coupling, a drive gear shaft, a first driven gear shaft, a second driven gear shaft, a sleeve, and an elastic retaining ring; The servo motor (121) is mounted on the motor mounting base (111) and connected to the drive gear shaft via a coupling; the drive gear (122), the first driven gear (123), and the second driven gear (124) are mounted via gear shafts and sleeves, and are axially fixed using shaft steps and elastic retaining rings; The servo motor (121) drives the drive gear (122), the drive gear (122) drives the first driven gear (123) and the second driven gear (124), and the driven gears drive the first external connecting rod (125) and the second external connecting rod (126) respectively; The first outer connecting rod (125) and the second outer connecting rod (126) are integrally machined with the extended connecting rod portions of the first driven gear (123) and the second driven gear (124), and the first fixed connecting rod (127) and the second fixed connecting rod (128) are connected to the linear bearing through standard hinge pins to form a linkage mechanism; the other end of the first fixed connecting rod (127) and the second fixed connecting rod (128) is hinged to the vertical mounting plate (112) through a pin.

4. The multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory according to claim 1, characterized in that, The pressure sensing system (13) includes a vertical extension platform (131), a ball joint platform (132), and a ball joint seat (133). There are two vertical extension platforms (131), which are connected to the first outer connecting rod (125) and the second outer connecting rod (126) respectively by bolts and nuts. A pressure sensor (135) is installed in the central groove of one of the vertical extension platforms (131). The ball joint seat (133) and the ball joint platform (132) are connected to the vertical extension platform (131) by four spring-loaded columns (136), and can move along the axial direction of the columns (136) to trigger or disengage the pressure sensor (135).

5. The multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory according to claim 1, characterized in that: The clamping and fixing unit (14) includes a ball joint ball (144), a ball joint connecting rod (143), a silicone mounting platform (141), and a lower fixing platform (146); the ball joint ball (144) is connected to one end of the ball joint connecting rod (143) by a thread and is housed in the ball chamber of the ball joint seat (133) and the ball joint seat platform (132); the other end of the ball joint connecting rod (143) is fixedly connected to the silicone mounting platform (141) by a stud and a nut; The lower fixed platform (146) is fixed to the vertical mounting plate (112) by an L-shaped fixing piece (147) and a stud nut, and the lower silicone plate is connected to it by a spring-loaded column.

6. The multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory according to claim 1, characterized in that: The stem cutting unit (15) includes a guide rod cylinder (151), a fixed blade (154), and a movable blade (153). The guide rod cylinder (151) is fixed to the cylinder platform (129) at the end of the second outer connecting rod (126) by a stud nut. The fixed blade (154) is also fixed to the cylinder platform (129) by a stud nut, and the cutting surfaces of the fixed blade (154) and the movable blade (153) slide in close contact, with a blade clearance of 0.5-0.8 mm. The movable blade (153) is fixed to the piston rod end of the guide rod cylinder (151).

7. The multifunctional robot non-destructive harvesting and grading system based on PSO optimized hybrid polynomial trajectory according to claim 1, characterized in that: The control center (16) is installed on the main body (20) of the harvesting robot. Its internal PLC control board is connected to the servo motor (121), pressure sensor (135), guide rod cylinder (151) and joint motor (24) through the lines embedded in the connecting rod.

8. A non-destructive citrus harvesting and grading method based on the multifunctional non-destructive citrus harvesting and grading system according to any one of claims 1-7, characterized in that, Includes the following steps: (1) The end effector (1) moves to a position below the target of the citrus to be harvested according to the planned trajectory; After power-on, the end effector (1) is in the first state. The extension connecting rods of the first driven gear (123) and the second driven gear (124) are at an angle of 90 degrees to the first external connecting rod (125) and the second external connecting rod (126). The pressure contact rod (134) in the pressure sensing system (13) is separated from the pressure sensor (135). The guide rod (152) of the guide rod cylinder (151) of the fruit stem cutting unit (15) is not extended. The end effector (1) is ready to pick citrus fruits. After the picking robot confirms the position of the fruit to be picked, the control center (16) calculates the motion control information required for the movement of the robotic arm (2) through trajectory planning. The robotic arm (2) moves under the action of the control center (16) and the joint motor (24) to send the end effector (1) to the bottom of the citrus to be picked. (2) End effector (1) Fixes the citrus fruits to be harvested and performs grading calculations; The servo motor (121) rotates forward and is driven by the drive system (12). The lower silicone contact surface (145) of the clamping and fixing unit (14) adaptively contacts different sizes of citrus fruits through the ball joint. The pressure contact rod (134) contacts the pressure sensor (135). When the force reading of the pressure sensor (135) reaches the preset clamping force threshold, the control center (16) controls the servo motor (121) to stop rotating. The end effector firmly clamps the fruit. This is the second state of the mechanism. The servo motor (121) feeds back the angle of rotation of the control center (16) in this movement. The control center (16) uses the database lookup table and interpolation method to back-calculate the transverse diameter of the citrus fruit through the rotation angle of the servo motor (121) and compares it with the pre-stored transverse diameter range of large, medium and small citrus fruits to determine the level of large, medium and small fruits and complete the online coarse grading of citrus fruits. (3) End effector (1) Cutting fruit stalks to harvest citrus fruits; Under the control center (16), the guide rod cylinder (151) is activated, the guide rod (152) extends, and the citrus fruit stalk is hooked into the V-shaped moving blade (153). During the return stroke of the moving blade (153) with the guide rod (152), the moving blade (153) and the fixed blade (154) cut off the fruit stalk and complete the harvesting of the citrus fruit. This is the third state of the mechanism. (4) Storage and transportation of citrus fruits after harvesting; Based on the grading of the fruit and the storage status of the corresponding grid fruit, the control center (16) uses trajectory planning to align the robotic arm (2) with the required grid position of the required fruit box (3). After the end effector (1) reaches the top of the corresponding grid of the fruit box, the control center (16) drives the servo motor (121) to reverse, and the clamping and fixing unit (14) releases the citrus, which falls into the grid. Then the mechanism returns to the first state, completing the picking, grading and storage of the fruit. When all the grids of the corresponding fruit box (3) are filled, the control center (16) will issue an alarm or automatically replace the corresponding fruit box (3).

9. The method for trajectory planning of citrus harvesting trajectory by the control center (16) according to claim 8, characterized in that, Includes the following steps: Step 1: Determine the relative positions of the robotic arm and the harvesting robot; Establish a motion model of the robotic arm (2), and use the link length, link rotation angle, link offset, and joint angle to model the DH coordinate system, and construct the coordinate transformation matrix between two adjacent links of the robotic arm (2); （1） In the formula, i is the link number. For rotation angle, The length of the link. Joint angle, This is the link offset. Let {i} be the homogeneous transformation matrix of coordinate system {i} relative to coordinate system {i-1}; the relative positional relationship between the manipulator and the harvesting robot is determined by the matrix transformation. Step 2: Based on the pre-obtained three-dimensional coordinates of the citrus and the starting position of the robotic arm, the target angles of each joint of the robotic arm (2) are solved in reverse. Determine the starting coordinates of the end effector (1) in the initial state of the robotic arm (2) and the target coordinates of the citrus fruits to be picked. Divide the planned picking path into three stages, corresponding to the acceleration stage, transition stage, and deceleration stage, respectively. , , The time period is determined, and key points along the path are generated. Using the DH model established in Step 1, the Cartesian coordinates are converted into target angles for each joint through inverse kinematics. Step 3: Perform polynomial interpolation on the three trajectories and calculate the required motion time for each segment; The following constraints apply to two adjacent trajectory segments: angular displacement, angular velocity, and angular acceleration are continuous; angular velocity and angular acceleration are lower than the rated values ​​corresponding to the joint motor (24); Connect the critical path segment points using a "3-5-3" mixed polynomial to generate a time-varying polynomial. The changing continuous trajectory uses cubic polynomial interpolation during the acceleration and deceleration phases and fifth polynomial interpolation during the transition phase, balancing solution time and solution quality. The interpolation function is constructed using 3-5-3 polynomial interpolation as follows: （2） In the formula, Let m be the motion time of each joint of the robotic arm (2). For robotic arm (2) , , Interpolation function for time period, These are the unknown coefficients in the interpolation function for each segment; Step 4: Use the PSO algorithm to optimize trajectory time based on the goal of minimizing time; The coefficients of the interpolation function are used as particle vectors. To optimize the generated trajectory, the PSO algorithm is used to iterate the interpolation function based on the time minimization objective. The update formulas for particle velocity and position are as follows: （3） （4） In the formula, The position of the particle. For the velocity of the particle, Number the particles. In particle dimension, This represents the current iteration number. For inertial weights, , A random number in [0, 1] , As a learning factor, For the optimal value of an individual particle, It is the global optimum of the population; The algorithm parameters are adaptively optimized based on the citrus grading results. When holding large fruits (with high physical inertia), the force is dynamically reduced. This encourages the algorithm to converge to a locally smooth trajectory earlier, reducing abrupt changes in the robotic arm's movement and ensuring smooth transport; when gripping small fruits, it increases... To enhance global optimization capabilities, shorten path planning time, and improve overall efficiency; Characterizes the local obstacle avoidance ability to circumvent fruit tree branches and trunks. It characterizes the global optimization ability to approach the target fruit box. It dynamically increases in areas with dense foliage. To enhance collision avoidance; increase size upon entering open areas. To speed up packing while ensuring both no damage and high efficiency; After the solution is completed, check the output of the solver, calculate the angular velocity and angular acceleration values ​​according to the interpolation function, and determine whether the preset constraint conditions are met. If the conditions are not met, return to solve again; if the conditions are met, output the solution results. Step 5: Solve for the motion parameters of each trajectory segment to complete the trajectory planning; After the solution is completed, the angular velocity and angular acceleration of each trajectory segment can be obtained from the angular displacement of each segment in the joint space. This allows for the continuous... The function is discretized into a series of specific angle commands according to the controller's cycle, and then sent to the joint motors to drive the robotic arm to move, thus completing the trajectory planning.

Images