Tea tender shoot picking device and tea dynamic picking method

Abstract

The invention discloses a tea leaf tender shoot picking device and a tea leaf dynamic picking method.The tea leaf tender shoot picking device comprises a gantry type crawler chassis, a visual detection device, a tea leaf picking device, a tea leaf collecting device and a control device; the visual detection device is used for acquiring a tea garden point cloud image and transmitting the tea garden point cloud image to the control device; the tea leaf picking device comprises a tea leaf picking mechanism and a tea leaf picking driving mechanism for driving the tea leaf picking mechanism to execute a tea leaf picking action; the control device is used for processing the received tea garden point cloud image to obtain a three-dimensional coordinate of an actual picking point of the tea tender shoots so as to control the tea picking driving mechanism to drive the tea picking mechanism to move to the position of the actual picking point to pick the tea tender shoots; and the picked tender shoots of the tea leaves are transferred into a tea leaf collecting device. According to the tea tender shoot picking device, accurate and lossless picking of tea tender shoots can be achieved, and the picking efficiency is higher.

Description

Technical Field

[0001] The invention belongs to the technical field of tea picking, and in particular relates to a tea bud picking device and a tea dynamic picking method. Background Art

[0002] Tea has a vast market in my country, with production increasing annually. The quality of tea production is directly linked to the quality of the leaves harvested, with high-quality tea generally focusing on harvesting young tea buds. Currently, tea production faces labor shortages, necessitating the mechanization of tea picking to improve efficiency.

[0003] Currently, the existing tea-picking robots can be divided into two types of terminal picking mechanisms according to their different picking methods: a pinching type terminal clamping mechanism and a finger-lifting type terminal clamping mechanism. The pinching type terminal clamping mechanism has knife-shaped fingers, which clamp and pick the tea leaves in a way of cutting the tea leaves during the picking process. However, this method is easy to damage the tender tea buds; the finger-lifting type terminal clamping mechanism has soft fingers with small size, which is used to simulate the force of human fingers, and relies on clamping and lifting to achieve tea picking. However, this method is easy to clamp empty tea leaves in a dynamic environment.

[0004] However, in terms of harvesting completeness, the finger-like picking type picking mechanism is more suitable for picking young tea buds; in order to improve the picking success rate of the finger-like picking type picking mechanism, the picking structure and picking method of the finger-like picking type picking mechanism also need to be optimized. Summary of the Invention

[0005] In order to overcome the deficiencies in the prior art, the present invention provides a tea bud picking device, which can achieve accurate and damage-free picking of tea buds with higher picking efficiency.

[0006] The second object of the present invention is to provide a dynamic tea picking method for a tea bud picking device.

[0007] The technical solution of the present invention to solve the above technical problems is:

[0008] A tea bud picking device comprises a gantry crawler chassis, a visual detection device arranged on the gantry crawler chassis, a tea picking device, a tea collecting device and a control device, wherein:

[0009] The visual detection device is used to obtain the point cloud image of the tea garden and transmit it to the control device;

[0010] The tea picking device includes a tea picking mechanism and a tea picking driving mechanism for driving the tea picking mechanism to perform tea picking actions, wherein:

[0011] The tea picking mechanism includes a distal main clamping finger, a distal auxiliary clamping finger and a clamping drive mechanism for driving the distal main clamping finger and the distal auxiliary clamping finger to open or close, wherein a flexible clamping block is provided on the distal main clamping finger; an elastic band is provided on the distal auxiliary clamping finger; the clamping drive mechanism includes a clamping seat and a proximal main clamping finger, a proximal auxiliary clamping finger and a clamping motor provided on the clamping seat; one end of the proximal main clamping finger is hinged to the distal main clamping finger, and the other end is rotatably connected to the clamping seat through a first rotating shaft; one end of the proximal auxiliary clamping finger is hinged to the distal auxiliary clamping finger, and the other end is rotatably connected to the clamping seat through a second rotating shaft; a gear set is provided on the first rotating shaft and the second rotating shaft; the clamping motor is installed on the clamping seat, and the main shaft of the clamping motor is connected to the first rotating shaft / second rotating shaft;

[0012] The control device is used to process the received tea garden point cloud image to obtain the three-dimensional coordinates of the actual picking point of the tea buds, thereby controlling the tea picking drive mechanism to drive the tea picking mechanism to move to the position of the actual picking point to pick the tea buds, and transfer the picked tea buds to the tea collection device.

[0013] Preferably, the gear set includes a first gear provided on the first rotating shaft and a second gear provided on the second rotating shaft, wherein the first gear and the second gear are meshed, and an outer diameter of the first gear is greater than an outer diameter of the second gear.

[0014] Preferably, a finger connecting rod is provided between the distal main clamping finger and the distal auxiliary clamping finger and the clamping seat respectively; one end of the finger connecting rod is hinged to the clamping seat, and the other end is hinged to the distal main clamping finger and the distal auxiliary clamping finger.

[0015] Preferably, the tea picking drive mechanism includes a connecting seat, an X-axis drive mechanism arranged on the connecting seat for driving the tea picking mechanism to rotate around the X-axis direction, and a Z-axis rotation drive mechanism for driving the X-axis drive mechanism and the tea picking mechanism connected to the X-axis drive mechanism to rotate around the Z-axis direction, wherein the X-axis drive mechanism includes a U-shaped seat, a connecting frame arranged in the U-shaped seat and an X-axis motor for driving the connecting frame to rotate; one end of the connecting frame is rotatably connected to the U-shaped seat, and the other end is connected to the clamping seat; the X-axis motor is mounted on the U-shaped seat, and the main shaft of the X-axis motor is connected to the connecting frame; the Z-axis rotation drive mechanism includes a connecting disk arranged on the connecting seat and a Z-axis motor for driving the connecting disk to rotate; the connecting disk is mounted on the U-shaped seat, and the main shaft of the Z-axis motor is connected to the connecting disk.

[0016] Preferably, the tea picking drive mechanism also includes a posture adjustment mechanism for adjusting the posture of the tea picking mechanism; the posture adjustment mechanism includes a connecting shell, multiple groups of adjusting parts arranged between the connecting shell and the connecting seat, and an adjusting motor for driving the adjusting parts to move, wherein the connecting shell is installed on the gantry crawler chassis; the adjusting parts are multiple groups, and correspondingly, the adjusting motors are also multiple groups; each group of adjusting parts includes a rocker arm and a connecting rod; the adjusting motor is installed in the connecting shell, the main shaft of the adjusting motor is connected to the rocker arm, the other end of the rocker arm is hinged to the upper end of the connecting rod, and the lower end of the connecting rod is hinged to the connecting seat.

[0017] Preferably, the tea collection device includes a collection frame and a fan arranged in the collection frame, wherein the collection frame is installed on the gantry crawler chassis and is provided with a collection port; the fan is arranged on the side wall of the collection frame opposite to the collection port; a filter is provided between the collection port and the fan; a discharge port is provided at the bottom of the collection frame, and the discharge port is connected to the collection box through a discharge pipe; the collection box is installed on the gantry crawler chassis.

[0018] Preferably, there are two groups of collecting boxes, and correspondingly, there are also two groups of discharging pipes; the feed ports of the two groups of discharging pipes are connected by a connecting plate; a servo is provided on the side wall of the collecting frame, and the output shaft of the servo is connected to the connecting plate; when one of the collecting boxes is full of tea buds, the servo drives the connecting plate to rotate, so as to cause the feed port of the discharging pipe connected to the other collecting box to dock with the discharge port of the collecting frame.

[0019] A dynamic tea picking method comprises the following steps:

[0020] Step S1: The gantry crawler chassis moves along the tea bushes in the tea garden. The visual inspection device takes a 60° downward shot of the tea bushes in front of it to obtain an RGB color image and a depth image of the tea garden. The depth image is then converted into a 3D point cloud based on the camera intrinsic parameters in the visual inspection device.

[0021] Step S2: After acquiring the RGB color image and 3D point cloud data of the tea garden, the YOLOv11 object detection framework is used to identify and select the tea buds in the RGB color image. After obtaining the detection frame of the tea buds, the center point of the detection frame is mapped from the 2D image coordinate system to the 3D point cloud space based on the projection transformation principle and using the pre-calibrated camera intrinsic parameter matrix and extrinsic parameter matrix. Priority planning for tea bud picking is performed based on the depth information corresponding to the center point of the detection frame.

[0022] Step S3: Using a straight-through filtering algorithm, the coordinate ranges in the X, Y, and Z axes are set according to the spatial distribution characteristics of the tea bushes in the tea garden to filter out the 3D point cloud data within the target area. An algorithm is then used to downsample the target point cloud. A statistical filter is then used to remove outliers. Finally, a Euclidean clustering extraction algorithm is used to segment the 3D point cloud data of the tea bushes in the tea garden to obtain separated branch point cloud clusters.

[0023] Step S4: For the tea bud with the highest priority, a traversal operation is performed in the segmented branch point cloud clusters based on the 3D coordinates of the center point of its detection frame. The Euclidean distance between this 3D coordinate point and all 3D coordinate points in each branch point cloud cluster is calculated, and the distance values are compared one by one. The branch point cloud with the closest 3D coordinate distance to the center point of the detection frame is selected as the reference point for picking the tea bud.

[0024] Step S5: By introducing the Kalman filter algorithm, the spatial probability distribution of the future moment of the tea bud picking reference point is predicted and optimized, the centroid of the spatial probability distribution is extracted as the optimal estimation point, and the approach speed of the tea picking device is dynamically adjusted based on the motion delay of the tea picking device and the covariance of the probability distribution. Finally, the compensated three-dimensional coordinates are used as the actual picking point of the tea bud;

[0025] Step S6: The control device generates a control instruction according to the actual picking coordinates, thereby controlling the tea picking device to pick the tea buds.

[0026] Preferably, in step S5, the steps of predicting and optimizing the spatial probability distribution of the tea bud picking points at future moments by using the Kalman filter algorithm are:

[0027] Step S501: Using the three-dimensional coordinates of the tea bud picking reference point output by the visual inspection device in the first frame as the initial observation value, the initial velocity component is estimated by position difference between adjacent frames to define the initial state of the tea leaves in the base coordinate system of the tea picking device; the initial state includes position, velocity, acceleration, and an initial covariance matrix is defined;

[0028] Step S502: Using the three-dimensional coordinates of the tea bud picking reference points output in real time by the visual inspection device as observation input, initializing the observation noise covariance parameters based on the visual measurement error characteristics, and implementing a prediction-update loop optimization using the state transfer matrix and the observation matrix to output the spatial probability distribution of the tea bud picking reference points at the future time.

[0029] Step S503: Extract the centroid of the spatial probability distribution as the optimal estimation point, and perform coordinate system transformation in combination with the motion delay of the tea picking device to realize coordinate correction of the three-dimensional coordinates of the optimal estimation point; predict the dynamic position of the tea picking position in the timing stage according to the covariance of the probability distribution, and adjust the approach speed of the tea picking device accordingly. After the above-mentioned correction of the predicted picking coordinates and adjustment of the approach speed, calculate the contact coordinates of the picking device and the predicted picking point as the actual picking point.

[0030] Preferably, in step S6, the tea picking device picks tea buds in the following steps:

[0031] The tea picking driving mechanism drives the tea picking mechanism to move to the actual picking point;

[0032] Based on the predicted movement direction of the tea buds, the clamping motor controls the distal main clamping fingers and the distal auxiliary clamping fingers to open, so that the elastic bands on the distal auxiliary clamping fingers face the incoming tea buds; when the elastic bands come into contact with the tea buds, the elastic bands suppress the disturbance of the tea buds;

[0033] The clamping motor controls the closure of the distal main clamping fingers and the distal auxiliary clamping fingers, wherein the flexible clamping blocks on the distal main clamping fingers fit with the elastic bands on the distal auxiliary clamping fingers to clamp the young tea buds; the tea clamping drive mechanism pulls the clamped young tea buds away from the branches and then throws them backwards into the tea collection device.

[0034] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0035] 1. During the picking process, the tea bud picking device of the present invention first obtains a point cloud image of the tea garden through a visual detection device and transmits it to a control device; the control device processes the received point cloud image of the tea garden to obtain the three-dimensional coordinates of the actual picking point of the tea buds, thereby controlling the tea picking drive mechanism to drive the tea picking mechanism to move to the position of the actual picking point to pick the tea buds, and transfers the picked tea buds to the tea collection device; thereby achieving accurate picking of the tea buds.

[0036] 2. The tea bud picking device of the present invention controls the distal main clamping fingers and the distal auxiliary clamping fingers to open according to the predicted movement direction of the tea buds, so that the elastic band on the distal auxiliary clamping fingers faces the upcoming tea buds; when the elastic band contacts the tea buds, the disturbance of the tea buds is suppressed by the elastic band; the clamping motor controls the distal main clamping fingers and the distal auxiliary clamping fingers to close, wherein the flexible clamping blocks on the distal main clamping fingers fit with the elastic band on the distal auxiliary clamping fingers to clamp the tea buds; the tea clamping drive mechanism pulls the clamped tea buds away from the branches and then throws them backwards into the tea collection device, thereby achieving damage-free picking of the tea leaves.

[0037] 3. The dynamic tea picking method of the present invention coordinates the trajectory of the tea picking device through Kalman prediction, takes into account the movement time of the tea picking drive mechanism and the tea picking mechanism, and predicts the tea picking point through the Kalman filter algorithm to compensate for the picking delay, thereby minimizing the phenomenon of tea leaves being picked empty by the tea clamping mechanism. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 and Figure 2 These are three-dimensional images of the tea bud picking device of the present invention from two different perspectives.

[0039] Figure 3 and Figure 4 These are three-dimensional images of the tea picking device from two different perspectives.

[0040] Figure 5 and Figure 6 These are three-dimensional images of the tea picking device from two different perspectives (part of the connecting seat is removed).

[0041] Figure 7 This is a three-dimensional diagram of the tea picking mechanism.

[0042] Figure 8 This is a three-dimensional diagram of the tea picking mechanism (without the clamping seat).

[0043] Figure 9 and Figure 10 These are three-dimensional images of the tea collection device from two different perspectives.

[0044] Figure 11 Schematic diagram for converting depth images into 3D point clouds.

[0045] Figure 12 Schematic diagram of the process of converting depth map into 3D point cloud.

[0046] Figure 13 Schematic diagram of the principle of selecting reference points for picking tea buds.

[0047] Figure 14Flowchart for statistical filtering and branch point cloud cluster acquisition.

[0048] Figure 15 This is a flow chart of dynamic prediction of tea buds based on Kalman filter algorithm.

[0049] Figure 16 This is the prediction result graph. DETAILED DESCRIPTION

[0050] The present invention will be described in further detail below with reference to the embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0051] See also Figures 1-16 The tea bud picking device of the present invention includes a gantry crawler chassis 1, a visual detection device 2 arranged on the gantry crawler chassis 1, a tea picking device 3, a tea collecting device 4 and a control device.

[0052] See also Figures 1-16 The visual detection device 2 is used to obtain the point cloud image of the tea garden and transmit it to the control device. The visual detection device 2 is a visual sensor, such as a binocular camera.

[0053] See also Figures 1-16 The tea picking device 3 includes a tea picking mechanism 9 and a tea picking driving mechanism for driving the tea picking mechanism 9 to perform tea picking actions.

[0054] See also Figures 1-16 The tea picking mechanism 9 includes a distal main clamping finger 902, a distal auxiliary clamping finger 903 and a clamping drive mechanism for driving the distal main clamping finger 902 and the distal auxiliary clamping finger 903 to open or close, wherein the distal main clamping finger 902 is provided with a flexible clamping block 904; the distal auxiliary clamping finger 903 is provided with an elastic belt 905; the clamping drive mechanism includes a clamping seat 901 and a proximal main clamping finger 908, a proximal auxiliary clamping finger 909 and a clamping motor provided on the clamping seat 901; one end of the proximal main clamping finger 908 is hinged to the distal main clamping finger 902, and the other end is rotatably connected to the clamping seat 9 through a first rotating shaft. 01; one end of the proximal auxiliary clamping finger 909 is hinged to the distal auxiliary clamping finger 903, and the other end is rotatably connected to the clamping seat 901 through a second rotating shaft; a gear set is provided on the first rotating shaft and the second rotating shaft; a clamping finger connecting rod 907 is provided between the distal main clamping finger 902 and the distal auxiliary clamping finger 903 and the clamping seat 901 respectively; one end of the clamping finger connecting rod 907 is hinged to the clamping seat 901, and the other end is hinged to the distal main clamping finger 902 and the distal auxiliary clamping finger 903; the clamping motor is installed on the clamping seat 901, and the main shaft of the clamping motor is connected to the first rotating shaft / second rotating shaft;

[0055] In this embodiment, the gear set includes a first gear 910 provided on the first rotating shaft and a second gear 911 provided on the second rotating shaft, wherein the first gear 910 and the second gear 911 are meshed, and the outer diameter of the first gear 910 is larger than the outer diameter of the second gear 911; in this way, when the first gear 910 rotates, the second gear 911 can be driven to rotate, so that the distal main clamping finger 902 and the distal auxiliary clamping finger 903 move simultaneously; since the outer diameter of the first gear 910 is larger than the outer diameter of the second gear 911, and since the linear velocity of the first gear 910 and the linear velocity of the second gear 911 are the same, the angular velocity of the second gear 911 will be greater than that of the first gear 910 angular velocity, and finally makes the movement speed of the distal auxiliary clamping finger 903 greater than the movement speed of the distal main clamping finger 902, so that the elastic band 905 on the distal auxiliary clamping finger 903 can first contact the tea buds, and the disturbance of the tea buds is suppressed by the elastic band 905, so as to position the tea buds and keep them in a static state; then, the clamping motor controls the distal main clamping finger 902 and the distal auxiliary clamping finger 903 to close, wherein the flexible clamping block 904 on the distal main clamping finger 902 fits with the elastic band 905 on the distal auxiliary clamping finger 903 to clamp the tea buds; the tea clamping drive mechanism drives the tea clamping mechanism 9 to move, so as to pull the clamped tea buds away from the branches, and then throw them backwards into the tea collection device 4.

[0056] See also Figures 1-16 The tea picking drive mechanism includes a connecting seat 701, an X-axis driving mechanism 8 provided on the connecting seat 701 for driving the tea picking mechanism 9 to rotate around the X-axis direction, and a Z-axis rotation driving mechanism 7 for driving the X-axis driving mechanism 8 and the tea picking mechanism 9 connected to the X-axis driving mechanism 8 to rotate around the Z-axis direction, wherein:

[0057] The X-axis drive mechanism 8 includes a U-shaped seat 801, a connecting frame 803 disposed in the U-shaped seat 801, and an X-axis motor 802 for driving the connecting frame 803 to rotate; one end of the connecting frame 803 is rotatably connected to the U-shaped seat 801, and the other end is connected to the clamping seat 901; the X-axis motor 802 is installed on the U-shaped seat 801, and the main shaft of the X-axis motor 802 is connected to the connecting frame 803;

[0058] The Z-axis rotation drive mechanism 7 includes a connecting disk 703 provided on the connecting seat 701 and a Z-axis motor 702 for driving the connecting disk 703 to rotate; the connecting disk 703 is mounted on the U-shaped seat 801, and the main shaft of the Z-axis motor 702 is connected to the connecting disk 703;

[0059] In this embodiment, the tea picking drive mechanism also includes a posture adjustment mechanism for adjusting the posture of the tea picking mechanism 9; the posture adjustment mechanism includes a connecting shell 601, multiple groups of adjustment parts arranged between the connecting shell 601 and the connecting seat 701, and an adjustment motor 602 for driving the adjustment parts to move, wherein the adjustment parts are multiple groups, and correspondingly, the adjustment motors 602 are also multiple groups; each group of adjustment parts includes a rocker rod 603 and a connecting rod 604; the adjusting motor 602 is installed in the connecting shell 601, and the main shaft of the adjusting motor 602 is connected to the rocker rod 603, and the other end of the rocker rod 603 is hinged to the upper end of the connecting rod 604, and the lower end of the connecting rod 604 is hinged to the connecting seat 701.

[0060] Through the coordinated movement of the above-mentioned X-axis drive mechanism 8, Z-axis rotation drive mechanism 7 and posture adjustment mechanism, the tea picking mechanism 9 is driven to move in all directions, so that the tea picking mechanism 9 can move to the actual picking point of the tea buds, pick the tea buds, and transfer the picked tea buds to the tea collection device 4 at the same time.

[0061] In this embodiment, there are two groups of connecting rods 604 in each group of adjusting members, and the two groups of connecting rods 604 are arranged in parallel.

[0062] See also Figures 1-16 The control device is used to process the received tea garden point cloud image to obtain the three-dimensional coordinates of the actual picking point of the tea buds, thereby controlling the tea picking drive mechanism to drive the tea picking mechanism 9 to move to the position of the actual picking point to pick the tea buds, and transfer the picked tea buds to the tea collection device 4.

[0063] See also Figures 1-16 The tea collection device 4 includes a collection frame 401 and a fan 404 arranged in the collection frame 401, wherein the collection frame 401 is installed on the gantry crawler chassis 1, and a collection port is provided on the collection frame 401; the fan 404 is arranged on the side wall of the collection frame 401 opposite to the collection port; a filter 402 is provided between the collection port and the fan 404; a discharge port 403 is provided at the bottom of the collection frame 401, and the discharge port 403 is connected to the collection box 5 through a discharge pipe 407; the collection box 5 is installed on the gantry crawler chassis 1.

[0064] In this embodiment, there are two groups of collecting boxes 5, and correspondingly, there are also two groups of discharging pipes 407; the feed ports of the two groups of discharging pipes 407 are connected by a connecting plate 406; a servo 405 is provided on the side wall of the collecting frame 401, and the output shaft of the servo 405 is connected to the connecting plate 406; when one of the collecting boxes 5 is full of young tea buds, the servo 405 drives the connecting plate 406 to rotate, so as to cause the feed port of the discharging pipe 407 connected to the other collecting box 5 to dock with the discharge port 403 of the collecting frame 401.

[0065] Through the above arrangement, the picked tea buds pass through the suction of the fan 404 and the obstruction of the filter 402, slide to the only discharge port 403 of the collection frame 401, and enter the collection box 5 along the discharge pipe 407, and the picking is completed; when one of the collection boxes 5 is full of tea buds, the servo 405 will drive the connecting plate 406 to rotate, so as to prompt the feed port of the discharge pipe 407 connected to the other collection box 5 to dock with the discharge port 403 of the collection frame 401, so that the picked tea buds will enter the corresponding collection box 5 along the discharge pipe 407.

[0066] See also Figures 1-16 The dynamic tea picking method of the present invention comprises the following steps:

[0067] Step S1: The gantry crawler chassis 1 moves along the tea bushes in the tea garden, and uses the visual detection device 2 to shoot the tea bushes in front at a 60° downward angle to obtain RGB color images and depth images of the tea garden. The depth image is converted into a 3D point cloud based on the camera intrinsic parameters in the visual detection device 2;

[0068] In this embodiment, the principle of converting the depth image into a 3D point cloud is as follows: Figure 11 As shown, where O c is the camera center, (x ω ,y ω , z ω ) is the real world coordinate point, z c is the depth from the target to the camera, is the image coordinate point, is the coordinate of the image center;

[0069] Therefore, the formula for converting the depth image from two-dimensional coordinates to three-dimensional point cloud is:

[0070]

[0071] Step S2: After acquiring the RGB color image and 3D point cloud data of the tea garden, the YOLOv11 object detection framework is used to identify and select the tea buds in the RGB color image. After obtaining the detection frame of the tea buds, the center point of the detection frame is mapped from the 2D image coordinate system to the 3D point cloud space based on the projection transformation principle and using the pre-calibrated camera intrinsic parameter matrix and extrinsic parameter matrix. Priority planning for tea bud picking is performed based on the depth information corresponding to the center point of the detection frame.

[0072] Step S3: Using a straight-through filtering algorithm, the coordinate ranges in the X, Y, and Z axes are set according to the spatial distribution characteristics of the tea bushes in the tea garden to filter out the 3D point cloud data within the target area. An algorithm is then used to downsample the target point cloud to reduce the point cloud density. A statistical filter is then used to remove outliers to achieve point cloud downsampling and denoising. Finally, a Euclidean clustering extraction algorithm is used to segment the 3D point cloud data of the tea bushes in the tea garden to obtain separated branch point cloud clusters.

[0073] Step S4: For the tea bud with the highest priority, a traversal operation is performed in the segmented branch point cloud clusters based on the 3D coordinates of the center point of its detection frame. The Euclidean distance between this 3D coordinate point and all 3D coordinate points in each branch point cloud cluster is calculated, and the distance values are compared one by one. The branch point cloud with the closest 3D coordinate distance to the center point of the detection frame is selected as the reference point for picking the tea bud.

[0074] Step S5: By introducing the Kalman filter algorithm, the spatial probability distribution of the tea bud picking reference point at the future moment is predicted and optimized, the centroid of the spatial probability distribution is extracted as the optimal estimation point, and the approach speed of the tea plucking device 3 is dynamically adjusted in combination with the motion delay of the tea plucking device 3 and the covariance of the probability distribution. Finally, the compensated three-dimensional coordinates are used as the actual picking point of the tea bud;

[0075] Step S6: The control device generates a control instruction according to the actual picking coordinates, thereby controlling the tea picking device 3 to pick the tea buds.

[0076] In step S5, the process of predicting and optimizing the spatial probability distribution of the future time of the tea bud picking point by using the Kalman filter algorithm is as follows: Figure 15 As shown, specifically:

[0077] Step S501: Using the three-dimensional coordinates of the tea bud picking reference point output by the visual inspection device 2 in the first frame as the initial observation value, the initial velocity component v0 is estimated by position difference between adjacent frames to define the initial state of the tea leaves in the base coordinate system of the tea picking device 3; the initial state includes position, velocity, acceleration, and an initial covariance matrix is defined;

[0078] In this embodiment, the motion, observation equations and Gaussian distribution of the Kalman filter algorithm are:

[0079]

[0080] Where: x k+1 is the state variable, u k is the input variable, w k is the process noise, A k is the state transfer matrix, B k is the control input matrix; z k is the output vector (the observation value obtained by the visual sensor), C k is the observation matrix (describing the state vector x k to the observed value z k The linear relationship between k is the measurement noise (error in the measurement process); Q k and R k are the process noise covariance matrix and the measurement noise covariance matrix respectively; the process noise w k and v k The mean is 0, and they are all Gaussian distributed random variables;

[0081] Velocity differential estimation:

[0082]

[0083] Set the state variables:

[0084]

[0085] Initial covariance estimate:

[0086]

[0087] Where: is the expected estimate of the state; is the state variable predicted at time k-1, x k-1 is the actual state variable at time k-1; is the covariance of the state errors.

[0088] Step S502: Using the three-dimensional coordinates of the tea bud picking reference point output in real time by the visual inspection device 2 as observation input, initializing the observation noise covariance parameter based on the visual measurement error characteristics, and implementing a prediction-update iterative loop optimization through the state transfer matrix and the observation matrix to output the spatial probability distribution of the tea bud picking reference point at the future time.

[0089] Among them, the state transfer matrix is:

[0090]

[0091] Where: The branch bud is equivalent to a simple pendulum motion k = θ / L, where k is the branch restoring force coefficient, g is the weight acceleration, L is the equivalent pendulum length, and c is the branch damping coefficient.

[0092] The prediction-update iterative process is:

[0093] State prediction for the next moment:

[0094]

[0095] Forecast state covariance matrix:

[0096]

[0097] Kalman gain calculation:

[0098]

[0099] Status forecast update:

[0100]

[0101] Update the covariance matrix:

[0102]

[0103] Where: is the state variable predicted at time k, A k-1 is the state transition matrix from time k-1 to time k, is the predicted state at time k-1, B k-1 is the control input matrix, u k-1 is the control input, w k-1 is the process noise; is the predicted covariance matrix, P k-1 is the covariance matrix of the previous moment, Q k-1 is the process noise covariance matrix; K k is the weighted Kalman gain that balances the predicted and actual observation values, C k To describe the measurement matrix that maps the state space to the observation space, R k is the observation noise covariance matrix that characterizes the observation error; z k is the actual observed value, is the predicted observation value, is the observed residual value that represents the difference between the predicted value and the actual observed value; k is the updated covariance matrix, K k C k is the product of the Kalman gain and the observation matrix.

[0104] Therefore, the implementation process of the entire Kalman filter algorithm is:

[0105] Based on the state vector changes of the tea buds in adjacent frames, a state transfer matrix A containing position, velocity and acceleration is constructed. k-1 ; Based on the state vector and state transfer matrix of the previous frame, the state of the tea buds in the next frame is predicted And based on the covariance P of the previous frame k-1 and process noise Q k-1 , get the predicted state covariance matrix at the current moment (i.e., the uncertainty or error range of the current state); with the observation matrix C k , current observation noise R k Covariance matrix and predicted covariance matrix Calculate the Kalman gain K k and the current forecast status and the current observation value z k Combine and get the corrected status update Finally, the Kalman gain K k Covariance with prediction Calculate the updated covariance matrix P k .

[0106] Step S503: Extract the centroid of the spatial probability distribution as the optimal estimation point, and perform coordinate system transformation in combination with the motion delay of the tea picking device to realize coordinate correction of the three-dimensional coordinates of the optimal estimation point; predict the dynamic position of the tea picking position in the timing stage according to the covariance of the probability distribution, and adjust the approach speed of the tea picking device accordingly. After the above-mentioned correction of the predicted picking coordinates and adjustment of the approach speed, calculate the contact coordinates of the picking device and the predicted picking point as the actual picking point.

[0107] In this embodiment, the centroid of the spatial probability distribution is extracted as the optimal estimation point of the tea bud position. The centroid comprehensively considers the probability of the tea bud appearing in each possible position, and can most representatively reflect the actual position of the tea bud, laying the foundation for subsequent precise operation: In view of the inevitable motion delay between the tea picking device 3 receiving the instruction and executing the picking action, in order to ensure that the tea picking device 33 can accurately pick the buds when they arrive at the target position, the coordinate system is transformed in combination with the delay characteristic, and the three-dimensional coordinates of the optimal estimation point are corrected and accurately converted to the actual position after considering the delay factor; the approach speed of the tea picking device 3 is dynamically adjusted according to the covariance of the probability distribution, Among them, the covariance intuitively reflects the uncertainty of the position of the tea buds: when the covariance is large, it means that the uncertainty of the position of the tea buds is high. In order to avoid mispicking, the tea picking device 3 will automatically slow down to allow sufficient time for accurate calibration; when the covariance is small, it indicates that the position of the tea buds is relatively certain, and the tea picking device 3 can appropriately increase the speed, thereby effectively improving the picking efficiency; after the dual optimization of the above-mentioned coordinate correction and speed adjustment, the final three-dimensional coordinates are the actual picking points; sending the coordinates to the picking device can achieve accurate picking of the tea buds.

[0108] In step S6, the tea picking device 3 picks tea buds in the following steps:

[0109] The tea picking driving mechanism drives the tea picking mechanism 9 to move to the actual picking point;

[0110] According to the predicted movement direction of the tea buds, the clamping motor controls the distal main clamping fingers 902 and the distal auxiliary clamping fingers 903 to open, so that the elastic bands 905 on the distal auxiliary clamping fingers 903 face the incoming tea buds; when the elastic bands 905 come into contact with the tea buds, the elastic bands 905 suppress the disturbance of the tea buds;

[0111] The clamping motor controls the closure of the distal main clamping finger 902 and the distal auxiliary clamping finger 903, wherein the flexible clamping block 904 on the distal main clamping finger 902 fits with the elastic band 905 on the distal auxiliary clamping finger 903 to clamp the tea buds; the tea clamping drive mechanism pulls the clamped tea buds away from the branches and then throws them backwards into the tea collection device 4.

[0112] The above is a preferred embodiment of the present invention, but the embodiment of the present invention is not limited to the above content. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. A tea bud picking device, characterized in that: It includes a gantry crawler chassis, a visual detection device, a tea picking device, a tea collecting device and a control device arranged on the gantry crawler chassis, wherein: The visual detection device is used to obtain the point cloud image of the tea garden and transmit it to the control device; The tea picking device includes a tea picking mechanism and a tea picking driving mechanism for driving the tea picking mechanism to perform tea picking actions, wherein: The tea picking mechanism includes a distal main clamping finger, a distal auxiliary clamping finger and a clamping drive mechanism for driving the distal main clamping finger and the distal auxiliary clamping finger to open or close, wherein a flexible clamping block is provided on the distal main clamping finger; an elastic band is provided on the distal auxiliary clamping finger; the clamping drive mechanism includes a clamping seat and a proximal main clamping finger, a proximal auxiliary clamping finger and a clamping motor provided on the clamping seat; one end of the proximal main clamping finger is hinged to the distal main clamping finger, and the other end is rotatably connected to the clamping seat through a first rotating shaft; one end of the proximal auxiliary clamping finger is hinged to the distal auxiliary clamping finger, and the other end is rotatably connected to the clamping seat through a second rotating shaft; a gear set is provided on the first rotating shaft and the second rotating shaft; the clamping motor is installed on the clamping seat, and the main shaft of the clamping motor is connected to the first rotating shaft / second rotating shaft; The control device is used to process the received tea garden point cloud image to obtain the three-dimensional coordinates of the actual picking point of the tea buds, thereby controlling the tea picking drive mechanism to drive the tea picking mechanism to move to the position of the actual picking point to pick the tea buds, and transfer the picked tea buds to the tea collection device.

2. The tea bud picking device according to claim 1, characterized in that: The gear set includes a first gear disposed on the first rotating shaft and a second gear disposed on the second rotating shaft, wherein the first gear is meshed with the second gear, and an outer diameter of the first gear is greater than an outer diameter of the second gear.

3. The tea bud picking device according to claim 1, characterized in that: A finger connecting rod is provided between the distal main clamping finger and the distal auxiliary clamping finger and the clamping seat respectively; one end of the finger connecting rod is hinged on the clamping seat, and the other end is hinged on the distal main clamping finger and the distal auxiliary clamping finger.

4. The tea bud picking device according to claim 1, characterized in that: The tea picking drive mechanism includes a connecting seat, an X-axis drive mechanism arranged on the connecting seat for driving the tea picking mechanism to rotate around the X-axis direction, and a Z-axis rotation drive mechanism for driving the X-axis drive mechanism and the tea picking mechanism connected to the X-axis drive mechanism to rotate around the Z-axis direction, wherein the X-axis drive mechanism includes a U-shaped seat, a connecting frame arranged in the U-shaped seat and an X-axis motor for driving the connecting frame to rotate; one end of the connecting frame is rotatably connected to the U-shaped seat, and the other end is connected to the clamping seat; the X-axis motor is installed on the U-shaped seat, and the main shaft of the X-axis motor is connected to the connecting frame; the Z-axis rotation drive mechanism includes a connecting disk arranged on the connecting seat and a Z-axis motor for driving the connecting disk to rotate; the connecting disk is installed on the U-shaped seat, and the main shaft of the Z-axis motor is connected to the connecting disk.

5. The tea bud picking device according to claim 4, characterized in that: The tea picking drive mechanism also includes a posture adjustment mechanism for adjusting the posture of the tea picking mechanism; the posture adjustment mechanism includes a connecting shell, multiple groups of adjustment parts arranged between the connecting shell and the connecting seat, and an adjustment motor for driving the adjustment parts to move, wherein the connecting shell is installed on the gantry crawler chassis; the adjustment parts are multiple groups, and correspondingly, the adjustment motors are also multiple groups; each group of adjustment parts includes a rocker arm and a connecting rod; the adjusting motor is installed in the connecting shell, the main shaft of the adjusting motor is connected to the rocker arm, the other end of the rocker arm is hinged to the upper end of the connecting rod, and the lower end of the connecting rod is hinged to the connecting seat.

6. The tea bud picking device according to claim 5, characterized in that: The tea collection device includes a collection frame and a fan arranged in the collection frame, wherein the collection frame is installed on the gantry crawler chassis and is provided with a collection port; the fan is arranged on the side wall of the collection frame opposite to the collection port; a filter is provided between the collection port and the fan; a discharge port is provided at the bottom of the collection frame, and the discharge port is connected to the collection box through a discharge pipe; the collection box is installed on the gantry crawler chassis.

7. The tea bud picking device according to claim 6, characterized in that: There are two groups of collecting boxes, and correspondingly, there are also two groups of discharge pipes; the feed ports of the two groups of discharge pipes are connected by a connecting plate; a servo is provided on the side wall of the collecting box, and the output shaft of the servo is connected to the connecting plate; when one of the collecting boxes is full of tea buds, the servo drives the connecting plate to rotate, so as to cause the feed port of the discharge pipe connected to the other collecting box to dock with the discharge port of the collecting box.

8. A dynamic tea picking method for the tea bud picking device according to any one of claims 1 to 7, characterized in that: The following steps are involved: Step S1: The gantry crawler chassis moves along the tea bushes in the tea garden. The visual inspection device takes a 60° downward shot of the tea bushes in front of it to obtain an RGB color image and a depth image of the tea garden. The depth image is then converted into a 3D point cloud based on the camera intrinsic parameters in the visual inspection device. Step S2: After acquiring the RGB color image and 3D point cloud data of the tea garden, the YOLOv11 object detection framework is used to identify and select the tea buds in the RGB color image. After obtaining the detection frame of the tea buds, the center point of the detection frame is mapped from the 2D image coordinate system to the 3D point cloud space based on the projection transformation principle and using the pre-calibrated camera intrinsic parameter matrix and extrinsic parameter matrix. Priority planning for tea bud picking is performed based on the depth information corresponding to the center point of the detection frame. Step S3: Using a straight-through filtering algorithm, the coordinate ranges in the X, Y, and Z axes are set according to the spatial distribution characteristics of the tea bushes in the tea garden to filter out the 3D point cloud data within the target area. An algorithm is then used to downsample the target point cloud. A statistical filter is then used to remove outliers. Finally, a Euclidean clustering extraction algorithm is used to segment the 3D point cloud data of the tea bushes in the tea garden to obtain separated branch point cloud clusters. Step S4: For the tea bud with the highest priority, a traversal operation is performed in the segmented branch point cloud clusters based on the 3D coordinates of the center point of its detection frame. The Euclidean distance between this 3D coordinate point and all 3D coordinate points in each branch point cloud cluster is calculated, and the distance values are compared one by one. The branch point cloud with the closest 3D coordinate distance to the center point of the detection frame is selected as the reference point for picking the tea bud. Step S5: By introducing the Kalman filter algorithm, the spatial probability distribution of the future moment of the tea bud picking reference point is predicted and optimized, the centroid of the spatial probability distribution is extracted as the optimal estimation point, and the approach speed of the tea picking device is dynamically adjusted based on the motion delay of the tea picking device and the covariance of the probability distribution. Finally, the compensated three-dimensional coordinates are used as the actual picking point of the tea bud; Step S6: The control device generates a control instruction according to the actual picking coordinates, thereby controlling the tea picking device to pick the tea buds.

9. The dynamic tea picking method according to claim 8, characterized in that: In step S5, the steps of predicting and optimizing the spatial probability distribution of the tea bud picking points at future moments by using the Kalman filter algorithm are as follows: Step S501: Using the three-dimensional coordinates of the tea bud picking reference point output by the visual inspection device in the first frame as the initial observation value, the initial velocity component is estimated by position difference between adjacent frames to define the initial state of the tea leaves in the base coordinate system of the tea picking device; the initial state includes position, velocity, acceleration, and an initial covariance matrix is defined; Step S502: Using the three-dimensional coordinates of the tea bud picking reference points output in real time by the visual inspection device as observation input, initializing the observation noise covariance parameters based on the visual measurement error characteristics, and implementing a prediction-update loop optimization using the state transfer matrix and the observation matrix to output the spatial probability distribution of the tea bud picking reference points at the future time. Step S503: extracting the centroid of the spatial probability distribution as the optimal estimation point, performing coordinate system transformation in combination with the motion delay of the tea picking device, and realizing coordinate correction of the three-dimensional coordinates of the optimal estimation point; The dynamic position of the tea picking position within the time series stage is predicted according to the probability distribution covariance, so as to adjust the approach speed of the tea picking device. After the above-mentioned correction of the predicted picking coordinates and adjustment of the approach speed, the contact coordinates between the picking device and the predicted picking point are calculated as the actual picking point.

10. The dynamic tea picking method according to claim 9, characterized in that: In step S6, the tea picking device picks tea buds in the following steps: The tea picking driving mechanism drives the tea picking mechanism to move to the actual picking point; Based on the predicted movement direction of the tea buds, the clamping motor controls the distal main clamping fingers and the distal auxiliary clamping fingers to open, so that the elastic bands on the distal auxiliary clamping fingers face the incoming tea buds; when the elastic bands come into contact with the tea buds, the elastic bands suppress the disturbance of the tea buds; The clamping motor controls the closure of the distal main clamping fingers and the distal auxiliary clamping fingers, wherein the flexible clamping blocks on the distal main clamping fingers fit with the elastic bands on the distal auxiliary clamping fingers to clamp the young tea buds; the tea clamping drive mechanism pulls the clamped young tea buds away from the branches and then throws them backwards into the tea collection device.

Images