A picking trajectory control method and system of a day lily picking robot

Abstract

The present application belongs to the technical field of agricultural picking control, and particularly relates to a picking trajectory control method and system for a day lily picking robot, the control method steps being as follows: S1: obtaining spatial position information of day lilies to be picked; S2: determining a target alignment pose of a side picking mechanism; S3: generating a safe approach trajectory between the initial position of the side picking mechanism and the target alignment pose; S4: planning the go time of the safe approach trajectory and synchronously adjusting the pose of an actuator; S5: controlling the position of a moving platform to remain relatively stable and driving the actuator to adjust to a picking pose; S6: controlling the actuator to complete day lily picking according to a preset picking mode; and S7: controlling the side picking mechanism to leave the target area along a safe evacuation trajectory. The method fully considers the picking tool orientation, generates a safe approach trajectory between the initial position of the side picking mechanism and the target alignment pose, synchronously plans the pose of the actuator, and improves picking accuracy.

Description

Technical Field

[0001] This invention belongs to the field of agricultural harvesting control technology, and in particular relates to a harvesting trajectory control method and system for a daylily harvesting robot. Background Technology

[0002] Daylilies, as an important economic crop, are widely cultivated in agricultural production. This crop is characterized by its large fruit length, significant structural flexibility, dispersed fruit growth location, and short harvesting window. Because daylilies are easily bent, damaged, or detached by external forces during the ripening stage, the harvesting process requires not only accurate picking location but also proper control of the approach direction and application of the harvesting tools to improve the harvesting success rate and reduce mechanical damage.

[0003] With the development of agricultural automation technology, harvesting robots are gradually being applied to the field of fruit and vegetable harvesting. Existing harvesting robots all adopt a top-harvesting structure, which is difficult to adapt to the side-harvesting requirements of daylilies. To address the aforementioned issues, Taiyuan University of Science and Technology proposed a hybrid side-harvesting robot on December 2, 2025 (application number 2025225617911). This robot uses a side-harvesting mechanism as the harvesting execution mechanism and a 4-PUU parallel structure to allow the harvesting tool to approach the target fruit from the side, improving its accessibility in complex plant environments. The 4-PUU parallel structure also features high structural rigidity, small end-effector error, and fast dynamic response. However, due to the structural differences between the 4-PUU parallel structure and serial robotic arms, its movement is characterized by limited workspace, strong motion coupling between branches, limited slider stroke, and complex posture constraints. Therefore, traditional harvesting trajectory control methods are difficult to apply directly. While some trajectories are continuous and meet end-effector motion requirements in Cartesian space, mapping them to the joint space of the parallel structure may cause the slider to exceed its stroke or become locally unreachable, resulting in the planned trajectory failing to execute.

[0004] In addition, existing harvesting trajectory control methods mainly focus on whether the end of the harvesting tool reaches the target point, ignoring the orientation and approach trajectory of the harvesting tool. Even if the positional error of the harvesting tool is small, improper orientation may lead to clamping failure or shearing deviation, resulting in low harvesting accuracy. At the same time, straight approach can easily cause the harvesting tool to interfere with surrounding obstacles, resulting in poor safety. Summary of the Invention

[0005] The purpose of this invention is to provide a harvesting trajectory control method and system for a daylily harvesting robot, which solves the problems that traditional harvesting trajectory control methods are difficult to apply directly to the harvesting robot, ignore the orientation and approach trajectory of the harvesting tool, and cause clamping failure or shearing deviation due to unreasonable orientation, resulting in low harvesting accuracy. Straight approach can easily cause the harvesting tool to interfere with surrounding obstacles, resulting in poor safety.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] A method for controlling the harvesting trajectory of a daylily harvesting robot is disclosed. The harvesting robot includes a tracked chassis, a gantry frame, a lifting platform, a side-harvesting mechanism, and a control system. The control system is fixed to the top of the gantry frame, and a visual navigation system is installed on the front side of the gantry frame. Two tracked chassis are arranged side by side, with their top ends fixed to the bottom ends of the gantry frame. The lifting platform is fixedly connected to the top of the gantry frame, and the side-harvesting mechanisms are fixed to their bottom ends. The side-harvesting mechanisms are symmetrically arranged. A hanger is fixedly connected below the lifting platform, and a depth camera is installed on the hanger, with each depth camera corresponding to one of the side-harvesting mechanisms. A collection mechanism and a drive motor are fixed to the top of the two tracked chassis, respectively. The side-harvesting mechanism includes a stationary platform, The system comprises guide rails, parallel chains, a moving platform, and actuators. The stationary platform is a square frame structure, fixed at its top to the bottom of the lifting platform. Hinge mounting platforms are installed at the four corners of the bottom surface. There are four identical guide rails, arranged symmetrically around the center of the stationary platform. The top of each guide rail is fixed to its corresponding hinge mounting platform, and the bottom of each guide rail is connected to the same fixed platform. Each guide rail is connected to a slider via a P-joint (sliding joint), and each slider is connected to a servo motor. There are four parallel chains, each hinged at its top to its corresponding slider. The bottom of each parallel chain is rotatably connected to the same moving platform, which has a rotation range of ±π / 4. The actuator is connected to the bottom of the moving platform, with its clamping end facing inwards. The specific steps are as follows:

[0008] S1: Obtain the spatial location information of the daylilies to be harvested, and determine the harvesting sequence based on the location distribution of the daylilies to be harvested;

[0009] S2: Based on the location of the daylily to be harvested, the size of the actuator, and the installation direction, determine the spatial correspondence between the moving platform of the side harvesting mechanism and the end of the actuator, and determine an alignment area near the daylily to be harvested that is suitable for the actuator to approach and act on the daylily; combine the movement range of the side harvesting mechanism, the distribution of obstacles around the daylily to be harvested, and the direction of action of the actuator to determine the target alignment posture of the side harvesting mechanism, which includes the alignment posture of the moving platform and the harvesting posture of the actuator;

[0010] S3: Generate a safe approach trajectory between the initial position of the side-harvesting mechanism and the target alignment posture. The safe approach trajectory includes a lifting motion away from the obstacle area, a transition motion towards the target area, and a descent motion towards the daylily. This reduces the risk of interference between the actuator, moving platform, daylily, and surrounding branches, fruit stalks, adjacent fruits, and the gantry. The area where the motion direction changes in the safe approach trajectory is smoothed to ensure a continuous transition between different motion segments of the side-harvesting mechanism, reducing speed abrupt changes, driving impacts, and end-effector vibrations at trajectory turning points.

[0011] S4: Perform outbound time planning for the safe approach trajectory to ensure that the side-harvesting mechanism moves smoothly along the safe approach trajectory and meets the low-impact harvesting requirements when approaching the daylily; during the movement of the side-harvesting mechanism along the safe approach trajectory, the actuator attitude is adjusted synchronously so that the actuator forms a preset initial alignment attitude when it reaches the target alignment area.

[0012] S5: When the side-harvesting mechanism reaches the target alignment area, the control platform position remains relatively stable and drives the actuator to adjust from the initial alignment posture to the harvesting posture, so that the actuator completes the alignment in a posture suitable for harvesting daylilies.

[0013] S6: After completing the posture alignment, the actuator is controlled to clamp and cut the daylily according to the preset picking method to complete the daylily picking; according to the kinematic constraints of the side picking mechanism, the planned dynamic platform posture is converted into the motion of each driving component, namely the slider and the drive motor, and it is determined whether the motion meets the requirements of the mechanism's working space, drive stroke, branch constraints and motion stability; if not, the target alignment posture, safe approach trajectory, smooth transition method or motion time is adjusted.

[0014] S7: After the daylily harvest is completed, the control side harvesting mechanism leaves the target area along the safe evacuation trajectory and restores the actuator to a posture suitable for subsequent harvesting operations; when there are unharvested targets, update the daylilies to be harvested and repeat the above trajectory planning and harvesting execution process until the target harvesting task is completed.

[0015] Preferably, the specific details of determining the harvesting order in step S1 are as follows:

[0016] Set an initial point P0. Assume that the daylily picking points include the first target point C1, the second target point C2, and the third target point C3. Establish a set of daylily picking points: , among which, the Each target point is represented as , The side-harvesting mechanism executes the harvesting tasks sequentially according to the order in the target point set: P0→C1→P0→C2→P0→C3→P0. That is, the side-harvesting mechanism first starts from the initial point P0, completes the harvesting of the first target point C1 and returns to P0, then starts from P0 to complete the harvesting of the second target point C2 and returns to P0 again; finally, it starts from P0 to complete the harvesting of the third target point C3 and finally returns to P0.

[0017] Preferably, the specific details of determining the target alignment pose of the side acquisition mechanism in step S2 are as follows:

[0018] (1) Clarify the geometric relationship between the center point of the moving platform and the end effector.

[0019] Let the center point of the moving platform be P, the end point of the actuator be M, and the length of the actuator be L. m The attitude angle of the moving platform around the vertical direction is Then the actuator end point and the moving platform center point satisfy the following: Therefore, it can be seen that when the side-harvesting mechanism is performing harvesting, the center point of the moving platform does not need to move directly to the target point, but rather to the alignment point near the target point, so that the actuator can point to the target point after it extends.

[0020] (2) Search the first Alignment points corresponding to each target point

[0021] For any target point C j With the target point as the center and the actuator length L as the length of the circle m Let be the radius, and establish a set of candidate alignment points in the horizontal plane. The candidate alignment points are represented as: ,in, For the search angle, the value range is: From the perspective of the search Perform counterclockwise discrete sampling to obtain multiple candidate alignment points. For each candidate alignment point, calculate the distance evaluation function between it and the initial point P0: Select the evaluation function The smallest angle is used as the optimal search angle. Given the same function value, the smaller angle is preferred because it is the optimal angle. , The value of the variable that minimizes the objective function is then the value of the first variable. The optimal alignment point corresponding to each target point is: By using the above search method, while ensuring that the actuator points to the target point, the alignment position of the center point of the moving platform is made as close as possible to the initial point, thus shortening the motion path and reducing the motion amplitude of the mechanism.

[0022] (3) Determine the picking posture angle

[0023] When the Alignment points of each target point Once determined, based on the target point With alignment point The orientation relationship between them determines the picking posture angle and calculates the target point. The relative direction of the target point in the horizontal plane with respect to the optimal alignment point, and taking this relative direction as the picking posture angle. The horizontal coordinate is Optimal alignment point The horizontal coordinate is The direction vector of the target point relative to the alignment point is - Then the picking posture angle Calculate using the following formula: ,in, It is a four-quadrant arctangent function used to determine the complete direction angle based on the two components of the direction vector, so that the extension direction of the actuator is consistent with the direction of the target point relative to the optimal alignment point, ensuring that the actuator points to the target point in the picking posture;

[0024] (4) Set the initial alignment attitude angle

[0025] To ensure the actuator has attitude adjustment margin after reaching the alignment pose corresponding to the target point, and to prevent the actuator from directly approaching the target point in the final picking posture, a preset attitude offset angle is set based on the picking posture angle to form the initial alignment posture angle. For the first... There are several target points, and their picking posture angles are set as follows: The preset attitude offset angle is The initial alignment attitude angle is ,in The settings are based on the shape of the target point, the structure of the actuator, and the requirements of the harvesting process. The value range is 10°-30°, when the side sampling mechanism moves to the alignment point. At that time, the actuator is in the final picking posture angle deflection. The initial alignment attitude is then maintained; subsequently, the position of the center point of the moving platform remains unchanged, and the actuator adjusts its attitude around the center point of the moving platform, so that the attitude angle changes from... Smooth change to This enables the actuator to be precisely aligned with the target point.

[0026] Preferably, the specific content of generating the safe approach trajectory in step S3 is as follows:

[0027] (1) Construct the outward trajectory in a gate-shaped pattern;

[0028] (2) Construct the circular arc trajectory;

[0029] (3) Optimization of circular arc trajectory;

[0030] (4) Parameterize the arc length of the complete trajectory.

[0031] Preferably, the specific details of step S4, which involves causing the actuator to form a preset initial alignment posture upon reaching the target alignment area, are as follows:

[0032] (1) Use a fifth-order polynomial to plan the outbound time of the side-collection mechanism;

[0033] (2) During the movement of the side-acquisition mechanism along the complete outward trajectory, the change in the attitude angle of the actuator is planned synchronously so that the side-acquisition mechanism is in the preset initial alignment posture when it reaches the optimal alignment point. The steps are as follows:

[0034] Let the initial attitude angle of the side-collection mechanism at the initial point be... , No. The initial alignment attitude angles corresponding to the target points are: Then the attitude angles during the outward trajectory motion are: ,in, It is a fifth-order polynomial time-scale function;

[0035] (3) Solve the inverse kinematics of the outgoing trajectory;

[0036] (4) Verify the reachability of the outbound trajectory;

[0037] (5) Calculate the velocity and acceleration of the slider.

[0038] Preferably, the specific details of harvesting daylilies in step S6 are as follows:

[0039] The side-picking mechanism maintains the center point position of the moving platform unchanged and controls the actuator to continue rotating along the preset direction to a predetermined picking angle. The rotational picking posture angle changes as follows: ,in, This refers to the movement time during the rotating harvesting stage. The preset picking rotation angle.

[0040] Preferably, the specific details of controlling the side-mounted sampling mechanism to leave the target area along the safe evacuation trajectory in step S7 are as follows:

[0041] (1) Construct the reverse return trajectory;

[0042] (2) Perform time planning, inverse kinematics solution and reachability verification on the return trajectory.

[0043] A daylily harvesting robot's harvesting trajectory control system includes a perception and positioning module, a vision recognition module, a trajectory control module, a chassis motion control module, a side-harvesting mechanism control module, and a system coordination and management module;

[0044] The perception and localization module connects to the system coordination and management module. It acquires information about the robot's own status and working environment through visual navigation, and at the same time, it combines the depth camera to acquire the robot's relative pose information in the field, providing basic data for subsequent control.

[0045] The visual recognition module connects to the perception and positioning module. It uses a depth camera to identify and locate the daylilies to be picked, extracts the three-dimensional spatial coordinates, posture information and pickable status of the daylilies to be picked, and outputs a set of target points to provide a basis for sorting and planning the picking tasks.

[0046] The trajectory control module, connected to the vision recognition module, is used to control the end-effector trajectory of the side-harvesting mechanism. It is the core module for achieving precise harvesting. Based on the spatial position of the daylily to be harvested, the attitude range of the moving platform, and the structural parameters of the actuator, this module generates the lateral approach trajectory, clamping trajectory, and cutting trajectory of the actuator. It also performs inverse kinematics solution on the parallel chain of the side-harvesting mechanism and outputs control commands for each drive joint, thereby achieving precise motion control of the actuator from the initial pose to the harvesting pose.

[0047] The tracked chassis control module, connected to the system coordination and management module, is used to control the movement of the tracked chassis, including the speed control, steering control, and driving stability control of the drive motor, so as to enable the robot to move forward, backward, and turn in the field, and to maintain consistency with the trajectory planning results.

[0048] The side-harvesting mechanism control module, connected to the trajectory control module, is used to control the actions of the side-harvesting mechanism, including parallel chain drive, dynamic platform attitude adjustment, and actuator picking action, to achieve precise execution of clamping and cutting actions;

[0049] The system coordination and management module, connected to the trajectory control module, is used to uniformly schedule and control the timing of each functional module, coordinate the cooperation between the tracked chassis movement and the picking action, avoid movement conflicts between walking and picking, and realize the coordinated operation of the whole machine.

[0050] Compared with the prior art, the beneficial effects achieved by the present invention are as follows:

[0051] (1) The picking trajectory control method determines the target alignment posture based on the position of the daylily to be picked, the structural size of the actuator and the motion characteristics of the side picking mechanism, so that the moving platform of the side picking mechanism does not need to move directly to the target point, but achieves accurate alignment of the daylily by adjusting the extension direction and posture of the actuator, thereby improving the matching accuracy between the actuator and the daylily, avoiding picking errors caused by tool length, installation offset or unreasonable posture, and improving picking accuracy;

[0052] (2) The picking trajectory control method generates a safe approach trajectory between the initial position of the side picking mechanism and the target alignment posture, so that the side picking mechanism first moves away from obstacles such as branches, fruit stalks, adjacent fruits or gantry frames, and then gradually approaches the daylily. Compared with the direct straight approach method, this method effectively reduces the risk of interference between the actuator, moving platform or daylily and surrounding obstacles, and improves the safety of the picking process.

[0053] (3) The picking trajectory control method synchronously plans the actuator posture during the trajectory movement, so that the actuator gradually adjusts to a suitable posture for picking as it approaches the daylily, and completes precise alignment after reaching the target alignment area, thereby improving the picking accuracy and picking quality.

[0054] (4) The picking trajectory control method fully considers the characteristics of the parallel robot, such as limited workspace, strong motion coupling between branches, limited slider stroke and complex posture change constraints, to prevent the slider from overtravel or local pose unreachable problems during the picking process, thus enhancing the actual executability of the trajectory. At the same time, it fully considers the change of picking tool orientation to improve picking accuracy. Attached Figure Description

[0055] Figure 1 This is a flowchart of the harvesting trajectory control method in an embodiment of the present invention;

[0056] Figure 2 This is a schematic diagram of the daylily harvesting robot according to an embodiment of the present invention;

[0057] Figure 3 This is a schematic diagram of the side-harvesting mechanism in the structure of the daylily harvesting robot according to an embodiment of the present invention;

[0058] Figure 4 This is a schematic diagram of the control system in the daylily harvesting robot structure according to an embodiment of the present invention.

[0059] Explanation of reference numerals in the attached drawings: 1. Tracked chassis; 2. Gantry frame; 3. Lifting platform; 4. Side acquisition mechanism; 41. Static platform; 42. Servo motor; 43. Guide rail; 44. Parallel chain; 45. Fixed platform; 46. Moving platform; 47. Actuator; 48. Slider; 5. Collection mechanism; 6. Drive motor; 7. Control system; 8. Visual navigation; 9. Depth camera; 10. Hanger. Detailed Implementation

[0060] The technical solution of the present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0061] A method for controlling the harvesting trajectory of a daylily harvesting robot, such as... Figure 2-3As shown, the harvesting robot includes a tracked chassis 1, a gantry frame 2, a lifting platform 3, a side-harvesting mechanism 4, and a control system 7. The control system 7 is fixed to the top of the gantry frame 2, and a visual navigation system 8 is installed on the front side of the gantry frame 2. The two tracked chassis 1 are arranged side by side, and the top ends of the tracked chassis 1 are fixed to the bottom ends of the gantry frame 2. The lifting platform 3 is fixedly connected to the top of the gantry frame 2. The side-harvesting mechanisms 4 are fixed to the bottom ends of the lifting platform 3. The side-harvesting mechanisms 4 are symmetrically arranged. The hanger 10 is fixedly connected to the bottom of the lifting platform 3. A depth camera 9 is installed on the hanger 10. The depth camera 9 corresponds to the side-harvesting mechanism 4. The collection mechanism 5 and the drive motor 6 are fixed to the top of the two tracked chassis 1 respectively. The side-harvesting mechanism 4 includes a static platform 41, a guide rail 43, a parallel chain 44, and a dynamic platform 45. The system comprises a platform 46 and an actuator 47. The stationary platform 41 is a square frame structure, fixed at the top to the bottom of the lifting platform 3. Hinges are mounted at the four corners of the bottom surface. There are four identical guide rails 43, arranged symmetrically around the center of the stationary platform 41. The top of each guide rail 43 is fixed to its corresponding hinge mounting platform, and the bottom of each guide rail 43 is connected to the same fixed platform 45. Each guide rail 43 is connected to a slider 48 via a P-joint, and each slider 48 is connected to a servo motor 12. There are four parallel chains 44, with their tops hinged to their corresponding sliders 48. The bottom of each parallel chain 44 is rotatably connected to the same moving platform 46. The moving platform 46 has a rotation range of ±π / 4. The bottom of the moving platform 46 is connected to the actuator 47, with the clamping end of the actuator 47 facing inwards. Figure 1 As shown, the specific steps are as follows:

[0062] S1: Obtain the spatial location information of the daylilies to be harvested, and determine the harvesting sequence based on the location distribution of the daylilies to be harvested.

[0063] Set initial point Let the target points for harvesting daylilies include the first target point C1, the second target point C2, and the third target point C3. Establish a set of target points: , among which, the Each target point is represented as , ,in:

[0064] ,

[0065] ,

[0066] ,

[0067] The side-harvesting mechanism executes the harvesting tasks sequentially according to the order in the target point set: P0→C1→P0→C2→P0→C3→P0. That is, the side-harvesting mechanism first starts from the initial point P0, completes the harvesting of the first target point C1 and returns to P0, then starts from P0 to complete the harvesting of the second target point C2 and returns to P0 again; finally, it starts from P0 to complete the harvesting of the third target point C3 and finally returns to P0.

[0068] S2: Based on the location of the daylily to be harvested, the size of the actuator, and the installation direction, determine the spatial correspondence between the moving platform of the side harvesting mechanism and the end of the actuator, and determine the alignment area near the daylily to be harvested that is suitable for the actuator to approach and act on the daylily; combine the movement range of the side harvesting mechanism, the distribution of obstacles around the daylily to be harvested, and the direction of action of the actuator to determine the target alignment posture of the side harvesting mechanism, which includes the alignment posture of the moving platform and the harvesting posture of the actuator.

[0069] (1) Clarify the geometric relationship between the center point of the moving platform and the end effector.

[0070] Let the center point of the moving platform be P, the end point of the actuator be M, and the length of the actuator be L. m The attitude angle of the moving platform around the vertical direction is Then the actuator end point and the moving platform center point satisfy the following: The length of the actuator is L. m =100mm,

[0071] Therefore, when the side-harvesting mechanism is performing harvesting, the center point of the moving platform does not need to move directly to the target point, but rather to an alignment point near the target point, so that the actuator can point to the target point after it extends.

[0072] (2) Search the first Alignment points corresponding to each target point

[0073] For any target point C j With the target point as the center and the actuator length L as the length of the circle m Let be the radius, and establish a set of candidate alignment points in the horizontal plane. The candidate alignment points are represented as: ,in, For the search angle, the value range is: From the perspective of the search Perform counterclockwise discrete sampling to obtain multiple candidate alignment points. For each candidate alignment point, calculate the distance evaluation function between it and the initial point P0: Select the evaluation function The smallest angle is used as the optimal search angle. Given the same function value, the smaller angle is preferred because it is the optimal angle. Then the first The optimal alignment point corresponding to each target point is: By using the above search method, while ensuring that the actuator points to the target point, the alignment position of the center point of the moving platform is made as close as possible to the initial point, thus shortening the motion path and reducing the motion amplitude of the mechanism.

[0074] (3) Determine the picking posture angle

[0075] When the Alignment points of each target point Once determined, based on the target point With alignment point The orientation relationship between them determines the picking posture angle and calculates the target point. The relative direction of the target point in the horizontal plane with respect to the optimal alignment point, and taking this relative direction as the picking posture angle. The horizontal coordinate is Optimal alignment point The horizontal coordinate is The direction vector of the target point relative to the alignment point is - Then the picking posture angle Calculate using the following formula: ,in, It is a four-quadrant arctangent function used to determine the complete direction angle based on the two components of the direction vector, so that the extension direction of the actuator is consistent with the direction of the target point relative to the optimal alignment point, ensuring that the actuator points to the target point in the picking posture;

[0076] (4) Set the initial alignment attitude angle

[0077] To ensure the actuator has attitude adjustment margin after reaching the alignment pose corresponding to the target point, and to prevent the actuator from directly approaching the target point in the final picking posture, a preset attitude offset angle is set based on the picking posture angle to form the initial alignment posture angle. For the first... There are several target points, and their picking posture angles are set as follows: The preset attitude offset angle is The initial alignment attitude angle is ,in ,therefore When the side sampling mechanism moves to the alignment point At that time, the actuator is in the final picking posture angle deflection. The initial alignment attitude is then maintained; subsequently, the position of the center point of the moving platform remains unchanged, and the actuator adjusts its attitude around the center point of the moving platform, so that the attitude angle changes from... Smooth change to This enables the actuator to be precisely aligned with the target point.

[0078] S3: Generate a safe approach trajectory between the initial position of the side-harvesting mechanism and the target alignment posture. The safe approach trajectory includes lifting motion away from the obstacle area, transition motion towards the target area, and descent motion towards the daylily. This reduces the risk of interference between the actuator, moving platform, daylily and surrounding branches, fruit stalks, adjacent fruits, and gantry. The area where the motion direction changes in the safe approach trajectory is smoothed to ensure continuous transition between different motion segments of the side-harvesting mechanism, reducing speed abrupt changes, driving impacts, and end vibrations at trajectory turning points.

[0079] (1) Construct the outward trajectory in a gate shape

[0080] The gate-shaped outward trajectory includes a vertically ascending segment, a horizontal transition segment, and a vertically descending segment connected in sequence. The preset safe lifting height is set as follows: Based on the initial point Constructing the ascending transition point:

[0081] ,

[0082] According to the Alignment points corresponding to each target point Construct the point above the target:

[0083] ,in, To preset a safe lifting height, ,

[0084] Therefore, the first The gate-shaped outbound trajectories corresponding to the target points pass through the following sequentially: → → → That is, the side sampling mechanism first starts from the initial point Ascend vertically to the transition point Within the raised safe height plane, by Move horizontally to a point above the target. Descend vertically to the optimal alignment point corresponding to the target point. , and For vertical movement segment, For horizontal motion segments, each trajectory can be viewed as a straight line in space. To enable the robotic arm to move along the given trajectory, interpolation processing is required for each straight line segment, discretizing the continuous trajectory into several end-effector pose points. For each straight line trajectory segment, a linear interpolation method is used to generate trajectory points. Assume the starting point of a certain trajectory segment is... The destination is Then any interpolation point in the trajectory can be written as:

[0085] ,in, For interpolation parameters, when At that time, the end is located at the starting point. ;when At that time, the end is located at the endpoint. ,along with As the coordinates of the robotic arm gradually change from 0 to 1, the end effector moves along a straight line from the starting point to the ending point. After obtaining the Cartesian coordinates of the portal trajectory, each end effector position is substituted into the inverse kinematics model of the robotic arm to solve for the corresponding joint angles. The portal trajectory itself is planned in actuator space, but it needs to be converted into a joint space trajectory during actual execution. In this way, the angle curves of each joint of the robotic arm changing over time are obtained, driving the end effector to move along the portal trajectory.

[0086] The gate-shaped trajectory is composed of multiple straight lines spliced ​​together. and At these corner locations, the trajectory direction will change abruptly. If the robotic arm moves directly along a broken line trajectory, the end-effector velocity direction will suddenly change at the corner, which can easily lead to abrupt changes in joint velocity and acceleration, causing motion shock. In simulation, this will manifest as an uneven joint angle curve, which may cause vibration, shock, or increased tracking error. It is necessary to add an arc trajectory transition at the corner to improve the stability of trajectory execution.

[0087] (2) Constructing the circular arc trajectory

[0088] A circular arc trajectory has continuously varying curvature. In Cartesian space, the circular arc trajectory is determined by the center, radius, starting angle, and ending angle.

[0089] Assume the arc trajectory lies in a certain plane, with the center at... , radius is The starting angle is The termination angle is Then the coordinates of any point on the arc can be expressed as: ,in, exist and The changes between them, when When the change is uniform, a series of circular arc trajectory points are obtained.

[0090] Although the trajectory points are located on the arc, if the number of discrete points is insufficient, the actual movement of the robotic arm will be close to the broken line motion, and the smoothness of the arc is insufficient. If the number of sampling points is simply increased, the trajectory accuracy can be improved, but the inverse kinematics calculation will also be increased, and the smoothness of velocity and acceleration in joint space may not be guaranteed. When the arc trajectory is connected to the straight trajectory, if no transition processing is performed, there may still be a sudden change in velocity direction, resulting in discontinuous movement of the robotic arm end effector.

[0091] (3) Optimization of circular arc trajectory

[0092] A cubic B-spline curve smoothing optimization method is adopted, combined with the re-interpolation of the trajectory discrete points, to make the circular trajectory smoother in space and have better continuity during the motion.

[0093] a. Generation of discrete points for the original circular arc: Generate discrete points for the original circular arc based on its geometric parameters. First, based on the center, radius, starting angle, and ending angle, use the parametric equations of the circular arc to obtain a set of initial trajectory points, describing the approximate shape of the arc.

[0094] The center of the circle is , radius is The starting angle is The termination angle is Then the coordinates of any point on the arc can be expressed as: ,in , angular range Discretize into m sampling points: Then the discrete points of the original circular arc are: ,in, The first arc represents the trajectory of the circle. There are 100 original discrete points.

[0095] The original discrete points describe the basic geometry of the circular arc trajectory, but adjacent points are usually still connected in a discrete manner. To further improve the continuity of the trajectory, B-spline smoothing optimization is required.

[0096] b. Cubic B-spline curve smoothing fitting: Using the original discrete points as control points or interpolation reference points, a cubic B-spline curve is introduced for fitting. The B-spline curve smooths the connection relationships between trajectory points while maintaining the overall shape of the arc.

[0097] Discretize the original circular arc points As control points or interpolation reference points, cubic B-spline curves are introduced for fitting and smoothing.

[0098] A cubic B-spline curve is represented as: ,in, For points on the B-spline curve, For curve parameters, As control points, For cubic spline basis functions, This represents the number of control points.

[0099] Discretize the original circular arc points Using the reference points for B-spline fitting, the smoothed curve is obtained. This curve, while maintaining the overall direction of motion and transition effect of the original circular arc, weakens the abrupt changes between discrete points, making the trajectory smoother in the corner area.

[0100] c. Optimize curve resampling: To ensure that the B-spline curve can better reflect the spatial distribution of the original circular arc trajectory, the chord length parameterization method can be used to determine the parameter values ​​corresponding to each discrete point.

[0101] Let the cumulative chord length of the original discrete points be:

[0102] , ,

[0103] The normalization parameter is then: ,in The total chord length of the original discrete point sequence of the circular arc is used. After parameterization with chord length, the distribution of curve parameters can correspond to the actual spatial distance between trajectory points, which helps to reduce the fitting error caused by uneven density of local trajectory points.

[0104] After completing the cubic B-spline curve fitting, the optimized curve is resampled to obtain a new sequence of circular arc trajectory points. Let the optimized B-spline curve be: , parameter range Re-divide the sampling points into r: The optimized trajectory points are: ,in, The th iteration after three B-spline curve optimizations and resampling A trajectory point.

[0105] The resampled trajectory points are no longer simple polyline connections between the original discrete points of the circular arc, but rather a continuous sequence of points on a smooth B-spline curve. This is achieved by appropriately increasing the number of sampling points. This improves the fineness of the trajectory point distribution, making the side-mining mechanism's moving platform move more smoothly along the arc transition section.

[0106] d. Continuous connection with preceding and following trajectory segments: In order to ensure that the optimized circular arc trajectory can be smoothly connected with the vertical and horizontal segments in the gate-shaped trajectory, the optimized curve should maintain positional continuity with adjacent trajectory segments at the starting and ending points, and should maintain tangential continuity as much as possible.

[0107] Let the starting point of the optimized circular arc trajectory be... The destination is The direction vector of the previous trajectory segment is The direction unit vector of the next trajectory segment is Then the optimized B-spline curve should simultaneously satisfy:

[0108] , ,

[0109] And make the tangent directions of the starting and ending points close to the directions of the preceding and following trajectory segments, respectively:

[0110] , ,in, To achieve better directional continuity between the optimized circular arc trajectory and the preceding and following straight line segments through the aforementioned constraints or approximate constraints, and to avoid the generation of new inflection points after optimization, in practical implementation, the positions of control points near the start and end points can be adjusted to ensure that the tangent direction at the end of the B-spline curve is consistent with or approximately consistent with the direction of the adjacent trajectory segment.

[0111] e. Perform inverse kinematics check on the circular arc segment after trajectory optimization: After completing the circular arc trajectory optimization and resampling, substitute the optimized trajectory points into the inverse kinematics model of the side sampling mechanism point by point to solve the corresponding joint variables or driving variables.

[0112] Let the optimized trajectory points be The corresponding attitude angle is: Then the first The joint variables corresponding to each trajectory point are represented as follows: ,in, This represents the inverse kinematics solution function of the side-access mechanism. For the first The joint variables or driving variables corresponding to each trajectory point are further calculated based on the joint variables corresponding to adjacent trajectory points to determine the joint velocity and acceleration.

[0113] If the sampling period is Then the joint velocity can be approximated as: The joint acceleration can be approximated as: Through inspection , , The changes in the trajectory are used to determine whether the optimized arc trajectory satisfies the motion constraints of the side-collection mechanism.

[0114] If both conditions are met:

[0115] ,

[0116] ,

[0117] ,

[0118] It is then assumed that the optimized circular arc trajectory meets the mechanism's execution requirements.

[0119] If excessive changes are found in local joint variables, joint velocities, or joint accelerations, further optimization can be achieved by increasing the number of sampling points, adjusting B-spline control points, increasing the radius of the circular arc transition, or extending the motion time of the circular arc segment, until the smoothness and executability requirements are met.

[0120] (4) Parameterize the arc length of the complete trajectory.

[0121] Let the first The complete outbound trajectory sequence corresponding to each target point is:

[0122] The arc length increment between adjacent trajectory points is ,

[0123] Define the cumulative arc length as: , Then the total length of the complete outbound trajectory corresponding to the i-th target point is: .

[0124] Therefore, the complete outgoing trajectory can be represented as a spatial trajectory function with arc length as the independent variable: ,in, .

[0125] By parameterizing the arc length, a corresponding relationship is established between the positional changes of the trajectory points and the path length, enabling the side-collection mechanism to move smoothly along the trajectory according to a given time pattern.

[0126] S4: Perform outbound time planning for the safe approach trajectory to ensure that the side-harvesting mechanism moves smoothly along the safe approach trajectory and meets the low-impact harvesting requirements when approaching the daylily; during the movement of the side-harvesting mechanism along the safe approach trajectory, the actuator attitude is adjusted synchronously so that the actuator forms a preset initial alignment attitude when it reaches the target alignment area.

[0127] (1) Use a fifth-order polynomial to plan the outbound time of the side-collection mechanism.

[0128] Let the total travel time be... Current time is The normalized time is: ,in: .

[0129] The following fifth-degree polynomial is used as the time scale function: ,

[0130] The scaling function simultaneously satisfies:

[0131] ,

[0132] Therefore, the speed and acceleration of the side sampling mechanism are both 0 or close to zero at the start and end points of the outward trajectory.

[0133] The path arc length at the current moment is Based on the current path arc length The center position of the eastern platform at the current moment is obtained through interpolation:

[0134] ,in, The cumulative arc length sequence for trajectory points. This is a sequence of points representing the complete outbound trajectory. The interpolation function is a cubic spline curve interpolation function, which is used to further improve the continuity of trajectory position changes.

[0135] (2) During the movement of the side-collection mechanism along the complete outward trajectory, the change in the attitude angle of the actuator is planned synchronously.

[0136] Let the initial attitude angle of the side-collection mechanism at the initial point be... , No. The initial alignment attitude angles corresponding to the target points are: Then the attitude angles during the outward trajectory motion are: ,in, It is a fifth-order polynomial time-scale function. This allows the side-mounted acquisition mechanism to synchronously complete attitude pre-adjustment during the position trajectory movement, ensuring that the actuator is in the initial alignment posture when it reaches the optimal alignment point.

[0137] (3) Solve the inverse kinematics of the outgoing trajectory.

[0138] Obtain the current position of the center point of the moving platform. and attitude angle Then, it is substituted into the inverse kinematics model of the side-collection mechanism to solve for the displacement of each driving component.

[0139] The side acquisition mechanism is a 4-PUU parallel side acquisition mechanism. The inverse kinematics solution process is used to convert the center position and attitude of the moving platform into the displacement of the four driving sliders: ,in, Indicates the first The driving displacement of each slider at the current moment.

[0140] The optimized trajectory in Cartesian space is converted into a slider displacement trajectory in the mechanism drive space, so that the trajectory is actually executed by the side-collection mechanism.

[0141] (4) Verify the reachability of the outbound trajectory.

[0142] Accessibility verification is performed on the mechanism pose corresponding to each trajectory point. For the ... The inverse kinematic discriminant of the branch chain is: Then it must satisfy At the same time, the slider displacement must satisfy: ,in, and The first The minimum and maximum allowable displacements of each slider.

[0143] When all branches meet the above conditions, the mechanism pose corresponding to the current trajectory point is determined to be reachable; when all trajectory points in the complete outgoing trajectory meet the above conditions, the complete outgoing trajectory is determined to be an executable trajectory.

[0144] If there are unreachable trajectory points, return to the trajectory planning step and adjust the arc transition radius, preset safe lifting height, optimal alignment point position or attitude offset angle until the trajectory meets the mechanism's accessibility requirements.

[0145] (5) Calculate the velocity and acceleration of the slider.

[0146] The velocity and acceleration of the driving displacement trajectory obtained by inverse kinematics solution are calculated.

[0147] For the The slider has the following speed: Its acceleration is: .

[0148] In discrete calculations, the finite difference method is used to calculate the slider velocity and acceleration.

[0149] If the slider speed or acceleration exceeds the preset threshold, adjust the outward travel time. The radius of the arc transition or the number of trajectory interpolation points are used to reduce the impact of the slider movement, and further determine whether the optimized trajectory meets the requirements of drive smoothness.

[0150] S5: When the side-harvesting mechanism reaches the target alignment area, the control platform position remains relatively stable and drives the actuator to adjust from the initial alignment posture to the harvesting posture, so that the actuator completes the alignment in a posture suitable for harvesting daylilies.

[0151] S6: After completing the posture alignment, the actuator is controlled to clamp and cut the daylily according to the preset picking method to complete the daylily picking; according to the kinematic constraints of the side picking mechanism, the planned dynamic platform posture is converted into the motion quantity of each driving component, and it is determined whether the motion quantity meets the requirements of the mechanism's working space, drive stroke, branch constraints and motion stability; if not, the target alignment posture, safe approach trajectory, smooth transition method or motion time is adjusted.

[0152] The side-picking mechanism maintains the center point position of the moving platform unchanged and controls the moving platform's attitude angle to continue rotating along the preset direction to a predetermined picking angle. The process involves rotating and picking or twisting the daylily buds. The rotating and picking process is as follows: ,in, This refers to the movement time during the rotating harvesting stage. To preset the picking rotation angle, The actuator rotates the daylily to complete the harvesting process.

[0153] S7: After the daylily harvest is completed, the control side harvesting mechanism leaves the target area along the safe evacuation trajectory and restores the actuator to a posture suitable for subsequent harvesting operations; when there are unharvested targets, update the daylilies to be harvested and repeat the above trajectory planning and harvesting execution process until the target harvesting task is completed.

[0154] (1) Construct the reverse return trajectory;

[0155] After completing the first After the harvesting action at each target point, the control side harvesting mechanism returns to the initial point along the reverse path of the outgoing trajectory. The return journey trajectory is as follows: →Vertical ascending segment→Reverse segment of the second circular arc transition segment→Reverse segment of the horizontal transition segment→Reverse segment of the first circular arc transition segment→Vertical descending segment→ That is, the return trajectory = reverse (outbound trajectory). The return trajectory adopts the reverse path of the outbound trajectory, so the return trajectory also has the characteristics of arc transition and smooth connection.

[0156] (2) Perform time planning, inverse kinematics solution and reachability verification on the return trajectory.

[0157] The return trajectory is processed in the same way as the outgoing trajectory, including arc length parameterization, quintic polynomial time programming, attitude recovery planning, inverse kinematics solution, and reachability verification.

[0158] a. Let the total length of the return trajectory be... The return journey time is Then the arc length of the return trajectory at the current moment is: During the return journey, the dynamic platform's attitude angles recover from the current attitude to the initial attitude angles. ,in, The posture angle after the picking action is completed. The initial attitude angle is [value]. When the return journey ends, the side-mounted acquisition mechanism returns to the initial point. and restore to the initial attitude angle. ,

[0159] (2) When the side sampling mechanism completes the first Pick the items at each target point and return to the initial point. Next, determine whether there are any unpicked target points in the target point set; if there are unpicked target points, then let: The above steps are repeated to complete trajectory planning, arc optimization, time planning, inverse kinematics solution, reachability verification, attitude alignment, rotation picking, and reverse return for the next target point; if all target points are picked, the multi-target continuous picking task ends.

[0160] like Figure 4 As shown, this embodiment also provides a daylily harvesting robot harvesting trajectory control system, including a perception and positioning module, a vision recognition module, a trajectory control module, a chassis motion control module, a side harvesting mechanism control module, and a system coordination and management module;

[0161] The perception and positioning module connects to the system coordination and management module. It obtains information about the robot's own status and working environment through visual navigation 8, and at the same time, it combines the depth camera 9 to obtain the robot's relative pose information in the field, providing basic data for subsequent control.

[0162] The visual recognition module connects to the perception and positioning module. It uses a depth camera 9 to identify and locate the daylilies to be picked, extracts the three-dimensional spatial coordinates, posture information and pickable status of the daylilies to be picked, and outputs a set of target points to provide a basis for sorting and planning the picking tasks.

[0163] The trajectory control module, connected to the vision recognition module, is used to execute the end trajectory control of the actuator 47 of the side-harvesting mechanism 4. It is the core module for achieving precise harvesting action. Based on the spatial position of the daylily to be harvested, the attitude range of the moving platform 46, and the structural parameters of the actuator 47, this module generates the lateral approach trajectory, clamping trajectory, and cutting trajectory of the actuator 47, and performs inverse kinematics solution on the parallel chain 44 of the side-harvesting mechanism 4, outputs control commands for each drive joint, and realizes precise motion control of the actuator 47 from the initial pose to the harvesting pose.

[0164] The tracked chassis control module is connected to the system coordination and management module and is used to control the movement of the tracked chassis 1, including the speed control, steering control and driving stability control of the drive motor 6, so as to enable the robot to move forward, backward and turn in the field and keep consistent with the trajectory planning results.

[0165] The side-collecting mechanism control module, connected to the trajectory control module, is used to control the actions of the side-collecting mechanism 4, including the driving of the parallel chain 44, the attitude adjustment of the moving platform 46, and the picking action of the actuator 47, so as to achieve the precise execution of the clamping and cutting actions.

[0166] The system coordination and management module, connected to the trajectory control module, is used to uniformly schedule and control the timing of each functional module, coordinate the relationship between the movement of the tracked chassis 1 and the picking action, avoid movement conflicts between walking and picking, and realize the coordinated operation of the whole machine.

[0167] Working principle: The control system 7 performs an initialization test on the entire harvesting robot to confirm that the tracked chassis 1, side harvesting mechanism 4, actuator 47, and collection mechanism 5 are all in normal working condition. Before entering the planting area, the height of the lifting platform 3 installed on the gantry 2 is manually adjusted to allow the robot to adapt to harvesting targets of different growth heights and expand the effective working range of the side harvesting mechanism 4. The control system 7 controls the tracked chassis 1 to enter the target planting area according to the operation path instructions, and the tracked chassis 1 enables the harvesting robot to move stably in the field.

[0168] During the movement, the two drive motors 6 drive the tracked chassis 1 to move. When the two drive motors 6 rotate in the same direction and at the same speed, the picking robot can move forward or backward in a straight line. When there is a speed difference or opposite direction between the two drive motors 6, the picking robot can turn to ensure that the robot can pass smoothly on muddy, soft or uneven ground in the field.

[0169] The control system 7 acquires the position parameters of the target through the depth camera 9 and transmits the target position information to the side harvesting mechanism 4. Four identical guide rails 43 are arranged symmetrically around the center of the static platform 41. The slider 48 moves linearly on the guide rails 43 and drives the moving platform 46 to generate spatial displacement and rotation within a certain angle range through the parallel chain 44. According to the spatial coordinates of the target, the control system 7 drives the parallel chains 44 to move in coordination, so that the moving platform 46 drives the actuator 47 to approach the target from the side.

[0170] When the actuator 47 reaches the predetermined picking position, the moving platform 46 adjusts its angle according to the posture of the picking target, so that the actuator 47 maintains a suitable relative position with the fruit and the fruit stem. The actuator 47 clamps the picking target from the side, completing the positioning and stable clamping. Since this picking method uses lateral clamping, it does not need to completely wrap the fruit. Therefore, when the target fruit is densely distributed, it reduces the interference between the actuator 47 and the surrounding fruit and branches, reducing the risk of accidental collision and damage. The cutting action is completed under the command of the control system 7.

[0171] After the cutting is completed, the side-harvesting mechanism 4 drives the actuator 47 to retract, and through the rotation of the moving platform 46, the harvested fruit is moved above the collecting mechanism 5. The actuator 47 releases the fruit, allowing it to fall into the collecting mechanism 5.

[0172] During continuous operation, the control system coordinates the tracked chassis 1, lifting platform 3, side-harvesting mechanism 4, and actuator 47 to sequentially complete the harvesting process of "walking and positioning—height adjustment—target recognition—lateral approach—clamping—cutting—retraction and transfer—placement into the collection mechanism." The harvesting robot continuously moves between the rows of target fruits and adjusts its overall position, harvesting height, and end effector posture in real time according to the changes in the position of the harvested target, thereby achieving automated harvesting of target fruits of different heights, densities, and spatial distributions.

Claims

1. A method for controlling the harvesting trajectory of a daylily harvesting robot, the harvesting robot comprising a tracked chassis, a gantry frame, a lifting platform, a side-harvesting mechanism, and a control system. The control system is fixed to the top of the gantry frame, and a visual navigation system is installed on the front side of the gantry frame. Two tracked chassis are arranged side-by-side, with their top ends fixed to the bottom of the gantry frame. A lifting platform is fixedly connected near the top of the gantry frame, and side-harvesting mechanisms are fixed to their bottom ends at symmetrically arranged. A hanger is fixedly connected below the lifting platform, and depth cameras are installed on the hanger, each corresponding to a side-harvesting mechanism. A collection mechanism and a drive motor are fixed to the top of each of the two tracked chassis. The side-harvesting mechanism includes... The system comprises a static platform, guide rails, parallel chains, a moving platform, and actuators. The static platform is a square frame structure, fixed at its top to the bottom of a lifting platform. Hinges are mounted on the bottom surface at each of the four corners. There are four identical guide rails, arranged symmetrically around the center of the static platform. The top of each guide rail is fixed to its corresponding hinge mounting platform, and the bottom of each guide rail is connected to the same fixed platform. Each guide rail is connected to a slider via a P-joint, and each slider is connected to a servo motor. There are four parallel chains, each hinged at its top to its corresponding slider. The bottom of each parallel chain is rotatably connected to the same moving platform, which has a rotation range of ±π / 4. The actuator is connected to the bottom of the moving platform, with its clamping end facing inwards. The system is characterized by... The specific steps are as follows: S1: Obtain the spatial location information of the daylilies to be harvested, and determine the harvesting sequence based on the location distribution of the daylilies to be harvested; S2: Based on the location of the daylily to be harvested, the size of the actuator, and the installation direction, determine the spatial correspondence between the moving platform of the side harvesting mechanism and the end of the actuator, and determine an alignment area near the daylily to be harvested that is suitable for the actuator to approach and act on the daylily; combine the movement range of the side harvesting mechanism, the distribution of obstacles around the daylily to be harvested, and the direction of action of the actuator to determine the target alignment posture of the side harvesting mechanism, which includes the alignment posture of the moving platform and the harvesting posture of the actuator; S3: Generate a safe approach trajectory between the initial position of the side-harvesting mechanism and the target alignment posture. The safe approach trajectory includes a lifting motion away from the obstacle area, a transition motion towards the target area, and a descent motion towards the daylily. This reduces the risk of interference between the actuator, moving platform, daylily, and surrounding branches, fruit stalks, adjacent fruits, and the gantry. The area where the motion direction changes in the safe approach trajectory is smoothed to ensure a continuous transition between different motion segments of the side-harvesting mechanism, reducing speed abrupt changes, driving impacts, and end-effector vibrations at trajectory turning points. S4: Perform outbound time planning for the safe approach trajectory to ensure that the side-harvesting mechanism moves smoothly along the safe approach trajectory and meets the low-impact harvesting requirements when approaching the daylily; during the movement of the side-harvesting mechanism along the safe approach trajectory, the actuator attitude is adjusted synchronously so that the actuator forms a preset initial alignment attitude when it reaches the target alignment area. S5: When the side-harvesting mechanism reaches the target alignment area, the control platform position remains relatively stable and drives the actuator to adjust from the initial alignment posture to the harvesting posture, so that the actuator completes the alignment in a posture suitable for harvesting daylilies. S6: After completing the posture alignment, the actuator is controlled to clamp and cut the daylily according to the preset picking method to complete the daylily picking; according to the kinematic constraints of the side picking mechanism, the planned dynamic platform posture is converted into the motion of each driving component, namely the slider and the drive motor, and it is determined whether the motion meets the requirements of the mechanism's working space, drive stroke, branch constraints and motion stability; if not, the target alignment posture, safe approach trajectory, smooth transition method or motion time is adjusted. S7: After the daylily harvesting is completed, control the side harvesting mechanism to leave the target area along the safe evacuation trajectory and restore the actuator to a posture suitable for subsequent harvesting operations; If there are unharvested daylilies, update the list of daylilies to be harvested and repeat the above trajectory planning and harvesting process until the target harvesting task is completed.

2. The harvesting trajectory control method for a daylily harvesting robot according to claim 1, characterized in that, The specific details of determining the harvesting sequence in step S1 are as follows: Set an initial point P0. Assume that the daylily picking points include the first target point C1, the second target point C2, and the third target point C3. Establish a set of daylily picking points: , among which, the Each target point is represented as , The side-harvesting mechanism executes the harvesting tasks sequentially according to the order in the target point set: P0→C1→P0→C2→P0→C3→P0. That is, the side-harvesting mechanism first starts from the initial point P0, completes the harvesting of the first target point C1 and returns to P0, then starts from P0 to complete the harvesting of the second target point C2 and returns to P0 again; finally, it starts from P0 to complete the harvesting of the third target point C3 and finally returns to P0.

3. The harvesting trajectory control method for a daylily harvesting robot according to claim 1, characterized in that, The specific details of determining the target alignment pose of the side acquisition mechanism in step S2 are as follows: (1) Clarify the geometric relationship between the center point of the moving platform and the end effector. Let the center point of the moving platform be P, the end point of the actuator be M, and the length of the actuator be L. m The attitude angle of the moving platform around the vertical direction is Then the actuator end point and the moving platform center point satisfy the following: Therefore, it can be seen that when the side-harvesting mechanism is performing harvesting, the center point of the moving platform does not need to move directly to the target point, but rather to the alignment point near the target point, so that the actuator can point to the target point after it extends. (2) Search the first Alignment points corresponding to each target point For any target point With the target point as the center and the actuator length as the radius, Let be the radius, and establish a set of candidate alignment points in the horizontal plane. The candidate alignment points are represented as: ,in, For the search angle, the value range is: From the perspective of the search Perform counterclockwise discrete sampling to obtain multiple candidate alignment points. For each candidate alignment point, calculate the distance evaluation function between it and the initial point P0: Select the evaluation function The smallest angle is used as the optimal search angle. Given the same function value, the smaller angle is preferred because it is the optimal angle. Then the first The optimal alignment point corresponding to each target point is: By using the above search method, while ensuring that the actuator points to the target point, the alignment position of the center point of the moving platform is made as close as possible to the initial point, thus shortening the motion path and reducing the motion amplitude of the mechanism. (3) Determine the picking posture angle When the Alignment points of each target point Once determined, based on the target point With alignment point The orientation relationship between them determines the picking posture angle and calculates the target point. The relative direction of the target point in the horizontal plane with respect to the optimal alignment point, and taking this relative direction as the picking posture angle. The horizontal coordinate is Optimal alignment point The horizontal coordinate is The direction vector of the target point relative to the alignment point is - Then the picking posture angle Calculate using the following formula: ,in, It is a four-quadrant arctangent function used to determine the complete direction angle based on the two components of the direction vector, so that the extension direction of the actuator is consistent with the direction of the target point relative to the optimal alignment point, ensuring that the actuator points to the target point in the picking posture; (4) Set the initial alignment attitude angle To ensure the actuator has attitude adjustment margin after reaching the alignment pose corresponding to the target point, and to prevent the actuator from directly approaching the target point in the final picking posture, a preset attitude offset angle is set based on the picking posture angle to form the initial alignment posture angle. For the first... There are several target points, and their picking posture angles are set as follows: The preset attitude offset angle is The initial alignment attitude angle is ,in The settings are based on the shape of the target point, the structure of the actuator, and the requirements of the harvesting process. The value range is 10°-30°, when the side sampling mechanism moves to the alignment point. At that time, the actuator is in the final picking posture angle deflection. The initial alignment attitude is then maintained; subsequently, the position of the center point of the moving platform remains unchanged, and the actuator adjusts its attitude around the center point of the moving platform, so that the attitude angle changes from... Smooth change to This enables the actuator to be precisely aligned with the target point.

4. The harvesting trajectory control method for a daylily harvesting robot according to claim 1, characterized in that, The specific details of generating the safe approach trajectory in step S3 are as follows: (1) Construct the outward trajectory in a gate-shaped pattern; (2) Construct the circular arc trajectory; (3) Optimization of circular arc trajectory; (4) Parameterize the arc length of the complete trajectory.

5. The harvesting trajectory control method for a daylily harvesting robot according to claim 1, characterized in that, The specific details of step S4, which involves causing the actuator to assume a preset initial alignment posture upon reaching the target alignment area, are as follows: (1) Use a fifth-order polynomial to plan the outbound time of the side-collection mechanism; (2) During the movement of the side-acquisition mechanism along the complete outward trajectory, the change in the attitude angle of the actuator is planned synchronously so that the side-acquisition mechanism is in the preset initial alignment posture when it reaches the optimal alignment point. The steps are as follows: Let the initial attitude angle of the side-collection mechanism at the initial point be... , No. The initial alignment attitude angles corresponding to the target points are: Then the attitude angles during the outward trajectory motion are: ,in, It is a fifth-order polynomial time-scale function; (3) Solve the inverse kinematics of the outgoing trajectory; (4) Verify the reachability of the outbound trajectory; (5) Calculate the velocity and acceleration of the slider.

6. The harvesting trajectory control method for a daylily harvesting robot according to claim 1, characterized in that, The specific steps for harvesting daylilies in step S6 are as follows: The side-picking mechanism maintains the center point position of the moving platform unchanged and controls the actuator to continue rotating along the preset direction to a predetermined picking angle. The rotational picking posture angle changes as follows: ,in, This refers to the movement time during the rotating harvesting stage. The preset picking rotation angle.

7. The harvesting trajectory control method for a daylily harvesting robot according to claim 1, characterized in that, The specific details of controlling the acquisition mechanism to leave the target area along the safe evacuation trajectory in step S7 are as follows: (1) Construct the reverse return trajectory; (2) Perform time planning, inverse kinematics solution and reachability verification on the return trajectory.

8. A harvesting trajectory control system for a daylily harvesting robot, characterized in that, It includes a perception and positioning module, a vision recognition module, a trajectory control module, a chassis motion control module, a side acquisition mechanism control module, and a system coordination and management module; The perception and localization module connects to the system coordination and management module. It acquires information about the robot's own status and working environment through visual navigation, and at the same time, it combines the depth camera to acquire the robot's relative pose information in the field, providing basic data for subsequent control. The visual recognition module connects to the perception and positioning module. It uses a depth camera to identify and locate the daylilies to be picked, extracts the three-dimensional spatial coordinates, posture information and pickable status of the daylilies to be picked, and outputs a set of target points to provide a basis for sorting and planning the picking tasks. The trajectory control module, connected to the vision recognition module, is used to control the end-effector trajectory of the side-harvesting mechanism. It is the core module for achieving precise harvesting. Based on the spatial position of the daylily to be harvested, the attitude range of the moving platform, and the structural parameters of the actuator, this module generates the lateral approach trajectory, clamping trajectory, and cutting trajectory of the actuator. It also performs inverse kinematics solution on the parallel chain of the side-harvesting mechanism and outputs control commands for each drive joint, thereby achieving precise motion control of the actuator from the initial pose to the harvesting pose. The tracked chassis control module, connected to the system coordination and management module, is used to control the movement of the tracked chassis, including the speed control, steering control, and driving stability control of the drive motor, so as to enable the robot to move forward, backward, and turn in the field, and to maintain consistency with the trajectory planning results. The side-harvesting mechanism control module, connected to the trajectory control module, is used to control the actions of the side-harvesting mechanism, including parallel chain drive, dynamic platform attitude adjustment, and actuator picking action, to achieve precise execution of clamping and cutting actions; The system coordination and management module, connected to the trajectory control module, is used to uniformly schedule and control the timing of each functional module, coordinate the cooperation between the tracked chassis movement and the picking action, avoid movement conflicts between walking and picking, and realize the coordinated operation of the whole machine.

Images