Tomato picking robot based on UWB navigation

Abstract

The invention relates to the technical field of agricultural automation equipment, and discloses a UWB navigation-based tomato picking robot, which comprises a crawler-type mobile chassis, a mounting rack, a control cabinet, a picking mechanical arm and an actuator assembly. A UWB positioning tag is installed on the top of the control cabinet, a global navigation and local obstacle avoidance system is constructed in combination with a laser radar, and the problem of large positioning deviation in a greenhouse weak signal environment is solved. The pitch angle of the picking mechanical arm is adjusted through a driving motor, and plane two-dimensional motion is achieved through a cross-shaped sliding table structure. The actuator assembly is based on eye-in-hand design, and an RGB-D depth camera, a multi-layer flexible suction cup and a rotating motor are integrated. During operation, the visual system guides the mechanical arm to accurately position, the pneumatic driver provides negative pressure to adsorb fruits, and the rotating motor drives the suction cup to twist to realize lossless separation of fruit stems. According to the invention, the navigation precision and hand-eye coordination of the picking robot are effectively improved, and automatic, low-loss and efficient picking of tomatoes is realized.

Description

Technical Field

[0001] This invention relates to the field of agricultural automation equipment technology, specifically to a tomato harvesting robot based on UWB navigation. Background Technology

[0002] With the development of facility agriculture, automated harvesting equipment is gradually replacing manual labor to reduce labor intensity. However, in actual greenhouse operations, tomato harvesting robots still face many technical challenges, which restrict their large-scale promotion and application.

[0003] First, the semi-enclosed structure and covering materials of greenhouses severely obstruct and attenuate satellite positioning signals, rendering general-purpose GNSS navigation systems unable to function properly inside. Existing greenhouse mobile platforms mostly employ odometry dead reckoning or magnetic strip navigation. Dead reckoning is susceptible to factors such as slippery ground and track slippage, leading to irreversible cumulative errors as the travel distance increases. Magnetic strip navigation requires pre-laid magnetic tracks, resulting in high deployment costs, fixed paths, and poor flexibility. In narrow planting rows, any positioning deviation can cause the robot to veer off-center, scrape crops, or get stuck in the passageway, making point-to-point movement difficult.

[0004] Secondly, in the fruit recognition and positioning stage, existing harvesting robots typically employ an "eye-outside-hand" layout, with the camera fixed to the robot's base. This results in a considerable distance between the visual sensor and the target, and the robot's view is easily obstructed by the robot itself during movement. Furthermore, the transformation chain from the camera coordinate system to the robot base coordinate system, and then to the end effector coordinate system is long. The transmission gaps and vibrations generated by the multi-joint movements of the robot amplify the cumulative errors in coordinate transformation, making it difficult for the end effector to accurately align with the target fruit, leading to grasping failure.

[0005] Finally, the operating method of the end effector directly affects the fruit integrity rate. Traditional rigid mechanical grippers are prone to damaging the tomato skin during clamping, while relying solely on negative pressure adsorption is often insufficient to overcome the connecting force of the ripe tomato stem. Although some solutions introduce rotational separation mechanisms, due to the lack of effective status feedback, they usually rely on force sensors to determine the separation time, or simply execute a preset fixed rotation angle. Force sensors are easily damaged in agricultural environments, and fixed angle control cannot adapt to individual differences, resulting in harvesting failure due to insufficient rotation or damage to the fruit flesh due to excessive rotation, making it difficult to simultaneously meet the requirements of low cost and damage-free harvesting. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a tomato-picking robot based on UWB navigation, which solves the problems of low positioning accuracy and large hand-eye coordination errors of existing greenhouse mobile platforms in greenhouse environments, as well as the difficulty of existing picking mechanisms in achieving both non-destructive grasping and reliable separation.

[0007] To achieve the above objectives, the present invention is implemented through the following technical solution: a tomato picking robot based on UWB navigation, comprising a tracked mobile chassis, a mounting frame and a control cabinet fixedly connected to the top of the tracked mobile chassis, a control system being installed inside the control cabinet, a picking robotic arm being installed inside the mounting frame, an actuator assembly being installed on the picking robotic arm, a UWB positioning tag being installed on the top of the control cabinet, and a drive motor being installed on the mounting frame;

[0008] The actuator assembly includes a mounting bracket, which is installed at the end of the harvesting robotic arm. An RGB-D depth camera, a multi-layer flexible suction cup, a rotary motor, and a pneumatic actuator are mounted on the mounting bracket. The pneumatic actuator is connected to the air circuit of the multi-layer flexible suction cup, and the output end of the rotary motor is connected to the multi-layer flexible suction cup.

[0009] Preferably, the harvesting robotic arm includes an X-axis guide rail, an X-axis slider, a Y-axis guide rail, and a Y-axis slider;

[0010] The X-axis slider is slidably mounted on the X-axis guide rail, the Y-axis guide rail is vertically fixedly connected to the X-axis slider, the Y-axis slider is slidably mounted on the Y-axis guide rail, and the actuator assembly is fixedly mounted on the front end of the Y-axis slider.

[0011] Preferably, a lidar is installed at the lower front end of the control cabinet, with the scanning plane of the lidar horizontally facing the direction of travel of the tracked mobile chassis. A collection box is horizontally installed at the upper rear end of the tracked mobile chassis, with the collection box located inside the mounting frame. An interactive display screen is provided on the top of the control cabinet.

[0012] Preferably, the RGB-D depth camera is fixed to the top plane of the mounting bracket and the optical axis of the lens is parallel to the axial direction of the multi-layer flexible suction cup. The multi-layer flexible suction cup is a corrugated tubular structure with multi-level buffer creases. The output shaft of the rotary motor is connected to the mounting base of the multi-layer flexible suction cup through a gear transmission structure.

[0013] Preferably, the control system includes a central controller, which establishes a communication connection with four UWB positioning base stations in the external environment;

[0014] The central controller is used to receive the distance measurement data between the UWB positioning tag and the four UWB positioning base stations, calculate the robot's global coordinates, and control the tracked mobile chassis to navigate to the target position based on the preset electronic map. The central controller is also used to control the picking robot arm and the actuator assembly to complete the picking action based on the data fed back by the RGB-D depth camera.

[0015] Preferably, the central controller calculates the robot's global coordinates by: establishing a positioning calculation model based on distance intersection; establishing distance constraint equations based on the measured distances and relative height differences between the UWB positioning tags and each UWB positioning base station; solving the distance constraint equations using the least squares method; and smoothing the solution results using the Kalman filter algorithm to output the estimated real-time coordinates of the robot in the global coordinate system at the current moment.

[0016] Preferably, the central controller controls the navigation of the tracked mobile chassis based on a preset electronic map as follows: the vertical distance from the estimated real-time coordinates to the navigation path line in the electronic map is calculated as the lateral deviation. Based on the PID control algorithm, the speed difference between the motors on the left and right sides of the tracked mobile chassis is adjusted according to the lateral deviation to drive the robot to stay on the center line between the rows. When the difference between the estimated real-time coordinates and the target plant coordinates is less than a preset position matching threshold, the robot is controlled to stop.

[0017] Preferably, the central controller controls the picking robot arm based on the data fed back from the RGB-D depth camera in the following manner: using a target detection model to identify ripe tomatoes in the image acquired by the RGB-D depth camera and extract pixel coordinates, converting the pixel coordinates into three-dimensional coordinates in the camera coordinate system based on the depth value and the camera intrinsic parameter matrix, using the hand-eye calibration matrix and the robot arm kinematics matrix to map the three-dimensional coordinates in the camera coordinate system into target coordinates in the robot arm base coordinate system, and generating multi-axis linkage interpolation commands.

[0018] Preferably, the central controller controls the actuator assembly to complete the picking action as follows: after the multi-layer flexible suction cup contacts the fruit, the pneumatic driver is activated. When the negative pressure value is detected to be lower than the adsorption determination threshold, the adsorption is determined to be successful. Then, the rotary motor is started to drive the fruit to rotate. At the same time, the load current of the rotary motor is monitored. When the load current drops by more than a preset threshold, the fruit is determined to separate and the rotation stops.

[0019] Preferably, the central controller is also used to execute local obstacle avoidance logic: when the lidar detects an obstacle in the virtual collision avoidance monitoring area and the distance is less than the deceleration threshold, it controls the robot to decelerate; when the obstacle does not completely block the road, it generates a local obstacle avoidance arc based on the local grid map and controls the tracked mobile chassis to detour.

[0020] This invention provides a tomato-harvesting robot based on UWB navigation. It has the following beneficial effects:

[0021] 1. This invention utilizes UWB positioning technology in conjunction with a Kalman filter algorithm to solve the problem of missing or unstable satellite positioning signals in enclosed environments such as greenhouses. By solving global coordinates through a distance intersection model and filtering out multipath interference, combined with PID path tracking control based on lateral deviation, the travel trajectory of the mobile chassis relative to the center line between rows can be corrected in real time, ensuring that the robot travels stably in narrow working channels and accurately reaches the target plant position, thus improving the environmental adaptability and positioning accuracy of the navigation system.

[0022] 2. This invention employs an RGB-D depth camera mounted on the end effector of a robotic arm, with the camera's optical axis set parallel to the axis of the flexible suction cup. This ensures that the visual perception area and the execution action area are highly aligned, simplifying the mapping process from the visual coordinate system to the end effector coordinate system. It also reduces the cumulative positioning error caused by the multi-stage transmission of the robotic arm joints. Combined with the linkage control of the Cartesian coordinate robotic arm, it enables rapid acquisition and precise alignment of the three-dimensional coordinates of the target fruit, thereby improving the success rate of harvesting operations.

[0023] 3. This invention designs a harvesting mechanism that combines pneumatic adsorption and rotational torque. It utilizes multi-layer flexible suction cups to adapt to the surface morphology of tomatoes of different sizes, avoiding damage to the fruit skin by rigid clamping. The separation status of the fruit stem is determined by monitoring the load current change of the rotating motor. When the load current is detected to drop suddenly due to the disappearance of resistance, the fruit is determined to be separated and the rotation is stopped. This control logic based on the physical load feedback of the motor replaces the expensive force sensor, achieving highly reliable non-destructive harvesting while reducing hardware costs. Attached Figure Description

[0024] Figure 1 This is a 3D view of the tomato harvesting robot based on UWB navigation according to the present invention;

[0025] Figure 2 This is a schematic diagram of the UWB positioning tag structure of the present invention;

[0026] Figure 3 This is a schematic diagram of the harvesting robotic arm structure of the present invention;

[0027] Figure 4 This is a schematic diagram of the actuator assembly structure of the present invention;

[0028] Figure 5 This is a block diagram of the control system principle of the present invention.

[0029] The components include: 1. Tracked mobile chassis; 2. Control cabinet; 3. Mounting frame; 4. Drive motor; 5. Harvesting robotic arm; 501. X-axis guide rail; 502. X-axis slider; 503. Y-axis guide rail; 504. Y-axis slider; 6. Actuator assembly; 601. Mounting bracket; 602. RGB-D depth camera; 603. Multi-layer flexible suction cup; 604. Rotary motor; 605. Pneumatic actuator; 7. Interactive display screen; 8. LiDAR; 9. UWB positioning tag; 10. Collection box. Detailed Implementation

[0030] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0031] Please see the appendix Figure 1 - Appendix Figure 5 This invention provides a tomato picking robot based on UWB navigation, including a tracked mobile chassis 1, a mounting frame 3 and a control cabinet 2 fixedly connected to the top of the tracked mobile chassis 1, a control system inside the control cabinet 2, a picking robotic arm 5 installed inside the mounting frame 3, an actuator assembly 6 installed on the picking robotic arm 5, a UWB positioning tag 9 installed on the top of the control cabinet 2, and a drive motor 4 installed on the mounting frame 3;

[0032] The actuator assembly 6 includes a mounting bracket 601, which is installed at the end of the harvesting robotic arm 5. The mounting bracket 601 houses an RGB-D depth camera 602, a multi-layer flexible suction cup 603, a rotary motor 604, and a pneumatic actuator 605. The pneumatic actuator 605 is connected to the air path of the multi-layer flexible suction cup 603, and the output of the rotary motor 604 is connected to the multi-layer flexible suction cup 603. A lidar 8 is installed at the lower front of the control cabinet 2, with its scanning plane horizontally facing the direction of travel of the tracked mobile chassis 1. A collection box 10 is horizontally installed at the upper rear of the tracked mobile chassis 1, located inside the mounting frame 3. An interactive display screen 7 is installed on the top of the control cabinet 2.

[0033] Specifically, the tracked mobile chassis 1 serves as the robot's mobile carrier, employing a dual-track differential drive structure. Each track is independently driven by two sets of high-torque brushless DC motors within the chassis. By increasing the robot's contact area with the ground, it can adapt to the soft, uneven soil inside the greenhouse, preventing slippage and sinking. A collection box 10 and a mounting frame 3 are horizontally mounted on the tracked mobile chassis 1. The mounting frame 3 is rigidly fixed to the front upper surface of the tracked mobile chassis 1 with bolts and is constructed from high-strength aluminum profiles in a portal frame shape, providing stable support for the upper structure. The collection box 10 is located at the rear of the chassis and is used to temporarily store harvested tomato fruits. The mounting frame 3, a gantry-type structure, is fixed above the tracked mobile chassis 1, located outside the collection box 10, and supports the control cabinet 2 and the operating mechanism.

[0034] Control cabinet 2 is installed at the front of the mounting frame 3. Inside control cabinet 2, a central controller, motor driver, and power module are integrated. An interactive display screen 7 is installed on the outer surface of control cabinet 2 for displaying system status and human-machine interaction. A LiDAR 8 is installed at the lower front of control cabinet 2, with its scanning plane horizontally facing the robot's direction of travel to detect obstacles in the path. A UWB positioning tag 9 is installed at the highest point of control cabinet 2, positioned above the typical height of a tomato plant to ensure that wireless signal transmission between the UWB positioning tag 9 and the external base station is not obstructed by the metal frame or crop branches. The UWB positioning tag 9 is connected to the central controller inside control cabinet 2 via a data cable, providing real-time global coordinate information of the robot.

[0035] A drive motor 4 is mounted on the crossbeam of the mounting frame 3, and its output shaft is connected to the base of the harvesting robotic arm 5. It drives the harvesting robotic arm 5 to perform overall pitching motion around a horizontal axis to adapt to tomato bunches of different heights. An actuator assembly 6 is mounted on the harvesting robotic arm 5. The actuator assembly 6 includes a mounting bracket 601, an RGB-D depth camera 602, a multi-layer flexible suction cup 603, a rotary motor 604, and a pneumatic actuator 605. The RGB-D depth camera 602 is fixed above the mounting bracket 601 and is used to acquire color images and depth point cloud data of the working area. The pneumatic actuator 605 is connected to the multi-layer flexible suction cup 603 via an air circuit to provide vacuum negative pressure. The rotary motor 604 is connected to the multi-layer flexible suction cup 603 via a drive mechanism to drive the multi-layer flexible suction cup 603 to rotate around its axis.

[0036] Please see the appendix Figure 1 Appendix Figure 3 and attached Figure 4The harvesting robotic arm 5 includes an X-axis guide rail 501, an X-axis slider 502, a Y-axis guide rail 503, and a Y-axis slider 504. The X-axis slider 502 is slidably mounted on the X-axis guide rail 501, the Y-axis guide rail 503 is vertically fixedly connected to the X-axis slider 502, the Y-axis slider 504 is slidably mounted on the Y-axis guide rail 503, and the actuator assembly 6 is fixedly mounted on the front end of the Y-axis slider 504.

[0037] Specifically, the harvesting robotic arm 5 adopts a cross-shaped rectangular coordinate slide structure, with the X-axis guide rail 501 serving as the base component and arranged horizontally. The X-axis slider 502 is mounted on the X-axis guide rail 501 and is driven by a servo motor via a lead screw, moving horizontally left and right along the X-axis guide rail 501. The Y-axis guide rail 503 is vertically fixed on the X-axis slider 502 and moves synchronously with it. The Y-axis slider 504 is mounted on the Y-axis guide rail 503 and can extend and retract along the Y-axis guide rail 503. This achieves precise two-dimensional positioning of the harvesting end in the horizontal plane.

[0038] Please see the appendix Figure 4 The RGB-D depth camera 602 is fixed to the top plane of the mounting bracket 601 and the optical axis of the lens is parallel to the axis of the multi-layer flexible suction cup 603. The multi-layer flexible suction cup 603 is a corrugated tubular structure with multi-level buffer folds. The output shaft of the rotary motor 604 is connected to the mounting base of the multi-layer flexible suction cup 603 through a gear transmission structure.

[0039] Specifically, the actuator assembly 6 is fixedly mounted on the front end of the Y-axis slider 504, including a mounting bracket 601, an RGB-D depth camera 602, a multi-layer flexible suction cup 603, a rotary motor 604, and a pneumatic actuator 605. The mounting bracket 601 serves as a connection and support, and its rear end is connected to the flange of the Y-axis slider 504. The RGB-D depth camera 602 is fixed to the top plane of the mounting bracket 601 with screws, and its lens optical axis faces forward, maintaining a fixed relative position with the axis of the multi-layer flexible suction cup 603. The integrated hand-eye design eliminates complex coordinate mapping errors.

[0040] The multi-layer flexible suction cup 603 is located at the front center of the mounting bracket 601. Made of silicone, it has a corrugated tubular structure with multi-level cushioning folds, allowing it to elastically deform upon contact with the fruit to adapt to tomatoes of different sizes and shapes, forming a sealed cavity. The pneumatic actuator 605 mainly includes a vacuum generator and a pneumatic control valve, installed inside or on the side of the mounting bracket 601. It connects to the inner cavity of the multi-layer flexible suction cup 603 via an air tube to generate a vacuum negative pressure, providing suction and gripping force.

[0041] A rotary motor 604 is fixed to a mounting bracket 601, and its output shaft is connected to the mounting base of a multi-layer flexible suction cup 603 via a gear transmission structure. When the pneumatic actuator 605 maintains negative pressure adsorption, the rotary motor 604 starts and outputs torque, causing the multi-layer flexible suction cup 603 and the adsorbed tomato fruit to rotate around its own axis. This rotational motion simulates the twisting action during manual harvesting, utilizing the low torsional strength at the connection between the tomato stem and fruit to achieve non-destructive separation of the fruit from the plant.

[0042] See attached document Figure 2 - Appendix Figure 5 The UWB positioning network deployment scheme mainly consists of four UWB positioning base stations distributed around the work area and UWB positioning tags 9 installed on the robot itself. The four UWB positioning base stations are fixedly installed on the four vertices of the rectangular work area inside the greenhouse, forming a positioning rectangle covering the entire crop planting ridge. The vertical installation height of each UWB positioning base station is set above the preset height of the tomato plant canopy, for example, more than 2.0 meters above the ground. This high-level deployment strategy ensures that there is always an unobstructed line-of-sight communication channel between the positioning base stations and the robot, effectively avoiding the absorption and attenuation of high-frequency wireless signals and multipath effects caused by the dense and tall tomato plant stems and leaves, thus guaranteeing the signal-to-noise ratio and stability of the positioning signal.

[0043] Four UWB positioning base stations are connected via wired Ethernet or a high-bandwidth wireless network to form a unified time synchronization network. The system sets the location of one of the base stations as the origin, and establishes a global Cartesian coordinate system with the long and short sides of the greenhouse as the X and Y axes, respectively. During system initialization, the relative physical coordinates of the four base stations are accurately calibrated using a laser rangefinder, and these coordinate parameters are stored in the robot's central controller as a static reference for subsequent position calculations.

[0044] The UWB positioning tag 9 is rigidly fixed to the center of the top surface of the robot control cabinet 2. This installation position is the highest point of the robot body structure, and there are no metal supports obstructing the view, ensuring that the UWB positioning tag 9 has the ability to transmit and receive signals 360 degrees omnidirectionally. During operation, the UWB positioning tag 9 sends pulse signals to the four surrounding UWB positioning base stations at a fixed refresh frequency. The system uses time-of-flight ranging or time-of-arrival method to measure the distance information between the UWB positioning tag 9 and the four base stations. The central controller receives the above ranging data, calculates it using a trilateration algorithm or least squares method, and outputs the planar coordinates of the robot body in the global coordinate system in real time. The coordinate data is directly used to drive the tracked mobile chassis 1 for straight-line navigation and path correction between planting rows.

[0045] The central controller stores a vectorized electronic map of the greenhouse environment. Based on the actual physical dimensions of the greenhouse, the electronic map defines the planting row of each tomato plant as a navigation path line and marks the theoretical growth position of each tomato plant as a discrete waypoint. In the electronic map's data structure, the navigation path line is described as a straight line equation in a global coordinate system, and the waypoint is described as an ordered set of two-dimensional coordinate points. ,in Represents the plant number. and Representing the first The horizontal and vertical coordinates of each plant in the global coordinate system.

[0046] In the positioning calculation stage, the central controller receives ranging data between UWB positioning tag 9 and four base stations. The system establishes a positioning calculation model based on distance intersection. Considering the flatness of the greenhouse ground and ignoring the impact of height changes on planar positioning, the positioning calculation follows the following distance constraint equation:

[0047] ;

[0048] in, Let be the planar coordinates of the robot body at the current moment; For the first The known fixed coordinates of UWB positioning base stations (where ); UWB positioning tag 9 and the Measured distance between UWB positioning base stations; This represents the relative height difference between UWB positioning tag 9 and the UWB positioning base station. Due to multipath interference in the greenhouse environment causing fluctuations in the original data, the central controller uses the least squares method to solve the above equations and combines it with the Kalman filter algorithm to refine the calculated data. After smoothing and filtering out high-frequency noise, the robot's real-time coordinates are estimated in the global coordinate system at the current moment.

[0049] Based on real-time coordinates The matching process with the electronic map is divided into two dimensions: path tracking matching and target point matching. In path tracking matching, the central controller calculates the vertical distance from the real-time coordinate point to the current navigation path line; this distance is the lateral deviation. The system employs a PID control algorithm to adjust the speeds of the motors on both sides of the tracked mobile chassis 1 based on the lateral deviation. The control law is as follows:

[0050] ;

[0051] ;

[0052] in, This is the lateral deviation. This is the speed correction amount; These are the proportional coefficient, integral coefficient, and differential coefficient, respectively. The preset baseline travel speed for the robot; Represents lateral deviation Regarding time The integral of the deviation is the cumulative amount of the deviation. Represents the system runtime variable; Represents lateral deviation Regarding time The derivative of the deviation, i.e., the rate of change of the deviation; The target rotational speed of the left track motor; This is the target rotational speed of the right-side track motor. Through the above control logic, when the robot deviates from the centerline, the central controller automatically generates a speed difference, driving the robot back to the centerline between the rows.

[0053] During target point matching, the system monitors the robot's displacement in the direction of travel in real time. The central controller continuously calculates the current real-time coordinates. Coordinates of the next target plant The difference between them. The system has a preset location matching threshold. When the conditions are met When the robot is positioned on the corresponding path, the map matching is considered successful. At this point, the central controller triggers a navigation completion signal, controlling the tracked mobile chassis 1 to decelerate and stop, while simultaneously activating the harvesting robotic arm 5 to enter the work preparation state. This coordinate matching-based navigation logic eliminates the cumulative errors that traditional visual navigation is prone to in environments with similar textures between rows, ensuring the robot's positioning repeatability in long-distance row operations.

[0054] Global path planning is primarily executed by the central controller based on greenhouse map information. Given the standardized ridge planting pattern used in tomato greenhouses, with narrow and straight aisles between rows, the global path planning strategy employs a linear planning algorithm based on topological nodes. The central controller extracts the centerline coordinates of each planting ridge from the electronic map, generating a series of parallel linear navigation trajectories. Before the task begins, the system determines the ridge number of the target plant based on the task list and plans the optimal travel sequence from the current stopping point to the entrance of the target ridge, and along the ridge centerline to the target work point. Using the planned path as the baseline trajectory, real-time tracking and correction are performed using global coordinates provided by the UWB positioning system.

[0055] The local obstacle avoidance logic primarily relies on the LiDAR 8 installed at the lower front of the control cabinet 2. As the robot travels along the planned path, the LiDAR 8 performs a two-dimensional scan of the horizontal plane ahead at a high-frequency rotation speed, acquiring distance point cloud data of the environmental contour. The central controller receives the point cloud data in real time and maps it onto a local grid map. Based on the physical width and kinematic constraints of the tracked mobile chassis 1, the system sets a rectangular virtual collision avoidance monitoring area along the travel direction on the local grid map.

[0056] The system executes a tiered safety response strategy based on the location and distance of obstacles within the virtual collision avoidance monitoring area. When the LiDAR 8 detects an obstacle (such as a fallen fruit box, workers, or other agricultural machinery) within the monitoring area, and the obstacle's distance is within a preset deceleration threshold range, the central controller outputs a deceleration command, controlling the tracked mobile chassis 1 to smoothly reduce its travel speed and continuously track the obstacle's dynamics. If the obstacle's distance continues to decrease and falls below the limit braking distance threshold, or if the obstacle is determined to be dynamically approaching a target, the central controller immediately triggers an emergency stop interrupt service, controlling the underlying motor driver to cut off power output and apply the brakes, bringing the robot to a stop in place until the obstacle is removed.

[0057] For small, static obstacles that do not completely block the road, the system has local detour planning capabilities. When the LiDAR 8 detects that the obstacle does not completely occupy the passage between rows, and the remaining space beside it is wider than the robot's passage width, the central controller temporarily suspends the UWB straight-line tracking control law and activates the local path planning algorithm. Based on the local grid map, the system generates a smooth local obstacle avoidance arc using the dynamic window method or the artificial potential field method. The tracked mobile chassis 1 detours around the obstacle along this arc. After the LiDAR 8 confirms that the path ahead is clear, the central controller refits the global straight-line path based on the coordinates fed back by the UWB positioning tag 9, and controls the robot to return to the center line between rows to continue traveling.

[0058] The central controller processes the image data acquired by the RGB-D depth camera 602 and converts it into motion control commands that the picking robotic arm 5 can execute.

[0059] The system employs an RGB-D depth camera 602 mounted on a mounting bracket 601 of the end effector assembly 6, moving synchronously with the harvesting robotic arm 5. The visual perception process begins with image acquisition; the RGB-D depth camera 602 acquires a color RGB image containing the tomato fruit and its corresponding depth image. The central controller internally deploys a deep learning-based object detection model (YOLO series algorithms or Faster R-CNN algorithm), trained and fine-tuned using a large number of mature tomato fruit samples. The central controller utilizes the object detection model to perform real-time inference on the RGB image, identifying the region of interest and bounding box of the mature tomato in the image pixel coordinate system, and extracting the pixel coordinates of the geometric center of the bounding box. .

[0060] To obtain the three-dimensional spatial information of the fruit, the system needs to convert the two-dimensional pixel coordinates into three-dimensional coordinates in the camera coordinate system. The central controller reads the corresponding pixels from the depth image. depth value This depth value represents the straight-line distance from the center of the fruit surface to the optical center of the camera. Based on the pinhole camera imaging model, the system uses the camera's intrinsic parameter matrix for back projection calculation. The conversion formula is as follows:

[0061] ;

[0062] ;

[0063] ;

[0064] in, The three-dimensional coordinates of the tomato's center in the camera coordinate system; The focal length of the camera in the horizontal and vertical directions; These are the coordinates of the principal point of the image (the intersection of the optical axis and the imaging plane); These are the x and y coordinates of the tomato's center in the image pixel coordinate system, respectively; This represents the depth value of the corresponding pixel in the depth image.

[0065] Through calculation, the central controller obtains the spatial position of the fruit relative to the RGB-D depth camera 602. However, the motion control of the harvesting robotic arm 5 depends on the coordinate system of the robotic arm base. Therefore, multi-source coordinate fusion and transformation must be performed to convert the coordinates in the camera coordinate system... The mapping is transformed to the coordinate system of the robotic arm base.

[0066] The coordinate fusion and transformation process involves two main transformation matrices: the hand-eye calibration matrix and the robotic arm kinematics matrix. The hand-eye calibration matrix describes the fixed positional relationship between the camera coordinate system and the end effector coordinate system (i.e., the coordinate system where the mounting bracket 601 is located). This matrix is ​​determined through calibration experiments after system assembly and remains unchanged. The robotic arm kinematics matrix describes the real-time positional relationship between the end effector coordinate system and the robotic arm base coordinate system. The hand-eye calibration matrix is ​​determined by the displacements of the X-axis slider 502 and the Y-axis slider 504, as well as the pitch angle of the drive motor 4.

[0067] set up Let the target coordinates of the fruit's center in the robot arm's base coordinate system be given. Let be the homogeneous coordinate vector in the camera coordinate system. Let be the homogeneous coordinate vector in the base coordinate system. The coordinate transformation chain follows the formula:

[0068] ;

[0069] in, A 4×4 hand-eye calibration matrix, including the camera's rotation and translation parameters; It is a 4×4 forward kinematics transformation matrix for the robotic arm. The central controller calculates and updates the values ​​in real time based on the encoder values ​​fed back from the servo motors of each axis.

[0070] After completing the coordinate transformation, the central controller obtains the precise absolute coordinates of the fruit relative to the robotic arm base. Subsequently, the system combines the length parameters of the multi-layer flexible suction cup 603 and the preset feed safety distance to generate the inverse kinematics solution of the picking robot arm 5, and plans the target motion of each joint (X-axis, Y-axis, pitch axis), driving the end effector assembly 6 to move precisely to the picking point. This coordinate processing technology, which integrates visual depth information and the kinematic state of the robot arm, ensures that the system can accurately locate and grasp the target in unstructured environments, regardless of the robot arm's posture.

[0071] The harvesting process begins with the precise approximation stage. After the visual perception system completes coordinate calculations, the central controller generates multi-axis linkage interpolation commands. The drive motor 4 adjusts the pitch angle of the harvesting robotic arm 5, aligning its vertical height with the target fruit bunch. Simultaneously, the X-axis slider 502 and Y-axis slider 504 work together to drive the actuator assembly 6 to approach the center of the target fruit along a straight trajectory. During the approximation process, the central controller employs a dual closed-loop control strategy based on position and velocity loops. When the front end of the multi-layer flexible suction cup 603 is less than a preset contact threshold (e.g., 5mm) from the target coordinates, the Y-axis slider 504 decelerates to a creeping speed until the flexible corrugated structure at the front end of the multi-layer flexible suction cup 603 makes physical contact with the surface of the tomato fruit.

[0072] After contact confirmation, the system enters the vacuum adsorption stage. The central controller sends an activation command to the pneumatic actuator 605, activating the vacuum generator and drawing air from the inner cavity of the multi-layer flexible suction cup 603 through the air tube. The system monitors the vacuum level changes in real time using a digital negative pressure sensor in the air circuit. The adsorption judgment threshold is set to -40 kPa. When the monitored negative pressure value reaches and stabilizes below this threshold within a specified time (e.g., 0.5 seconds), adsorption is considered successful, and the system transitions to the separation stage. If the threshold is not reached within the time limit, the system determines that adsorption has failed, and the central controller will control the robotic arm to fine-tune its position and trigger a retry logic.

[0073] The separation stage is crucial for achieving damage-free fruit harvesting. With the pneumatic actuator 605 continuously operating, the central controller starts the rotary motor 604. The rotary motor 604 drives the multi-layer flexible suction cup 603 and the adsorbed fruit to rotate around the central axis of the suction cups according to a preset speed curve (S-shaped acceleration / deceleration curve). The rotation applies torque to the connection between the tomato stem and the fruit. Because the torsional shear strength at the connection is much lower than the tensile strength of the stem itself, the fruit will separate from the stem after rotating a specific angle (typically 360 to 720 degrees). During this process, the central controller simultaneously monitors the current load of the rotary motor 604. When a momentary drop in the load current is detected, it indicates that the fruit has been successfully separated, and the rotary motor 604 immediately stops rotating and maintains its current position.

[0074] Finally, the system enters the recovery and release phase. After the fruit is separated, the central controller drives the Y-axis slider 504 to retract rapidly, causing the actuator assembly 6 to return to a safe area. Subsequently, the drive motor 4 lifts the harvesting robotic arm 5 upwards, and in conjunction with the movement of the X-axis slider 502, positions the actuator assembly 6 directly above the collection box 10. At this point, the central controller sends a shutdown command to the pneumatic actuator 605, and the solenoid valve switches the air path to allow positive pressure atmospheric air to pass through, disrupting the vacuum environment within the multi-layer flexible suction cup 603. After losing its suction force, the tomato fruit falls into the collection box 10 under gravity. The system then resets the state of each axis, preparing for the next harvesting cycle. The entire timing control process is scheduled by the central controller's real-time operating system at millisecond-level cycles, ensuring strict synchronization and logical interlocking of the actions of each actuator.

[0075] Working principle: First, the robot is moved to the greenhouse working area. The central controller in control cabinet 2 is activated and communicates with four preset UWB base stations in the greenhouse via UWB positioning tag 9 to calculate the robot's precise position in the greenhouse coordinate system. The central controller loads a preset electronic map, which contains the planting row positions and theoretical coordinate information of the tomato plants.

[0076] Subsequently, the central controller plans the travel path based on the coordinates of the target plant. The tracked mobile chassis 1 receives instructions and travels along the crop rows. During travel, the central controller reads the coordinate data fed back by the UWB positioning tag 9 at a high frequency, calculates the lateral deviation and heading angle deviation, and corrects the speed difference between the left and right wheels of the tracked mobile chassis 1 in real time to keep the robot traveling in a straight line. At the same time, the LiDAR 8 continuously monitors the environment ahead. If an obstacle is detected at a distance less than a safety threshold, the central controller controls the robot to stop urgently or execute obstacle avoidance logic.

[0077] Then, when the UWB positioning data shows that the robot has reached the coordinate range of the target plant, the tracked mobile chassis 1 stops. The central controller controls the drive motor 4 to adjust the pitch angle of the harvesting robotic arm 5 and controls the harvesting robotic arm 5 to extend, delivering the actuator assembly 6 to the vicinity of the crop canopy. The RGB-D depth camera 602 acquires images of the tomato bunches, identifies mature fruits, and calculates their three-dimensional spatial coordinates relative to the robotic arm base.

[0078] The central controller drives the harvesting robotic arm 5 to move according to the calculated fruit coordinates, aligning the central axis of the multi-layer flexible suction cup 603 with the center of the fruit and bringing it into contact with the fruit surface. The pneumatic system activates negative pressure, allowing the multi-layer flexible suction cup 603 to adhere to the fruit. Subsequently, the pneumatic actuator 605 actuates, causing the multi-layer flexible suction cup 603 and the fruit to rotate, separating the fruit from the stem through torsional torque.

[0079] After the fruit is separated, the picking robotic arm 5 retracts and rotates the actuator assembly 6 above the collection box 10. The pneumatic system cuts off the negative pressure, and the fruit falls into the collection box 10. The robot then enters the next cycle, continuing to pick other mature fruits from the current plant, or moving to the next plant according to navigation instructions.

Claims

1. A tomato picking robot based on UWB navigation, characterized in that, The system includes a tracked mobile chassis (1), on which a mounting frame (3) and a control cabinet (2) are fixedly connected. The control cabinet (2) contains a control system. A harvesting robotic arm (5) is installed inside the mounting frame (3). An actuator assembly (6) is installed on the harvesting robotic arm (5). A UWB positioning tag (9) is installed on the top of the control cabinet (2). A drive motor (4) is installed on the mounting frame (3). The actuator assembly (6) includes a mounting bracket (601) which is mounted on the end of the harvesting robotic arm (5). The mounting bracket (601) is equipped with an RGB-D depth camera (602), a multi-layer flexible suction cup (603), a rotary motor (604), and a pneumatic actuator (605). The pneumatic actuator (605) is connected to the air circuit of the multi-layer flexible suction cup (603), and the output end of the rotary motor (604) is connected to the multi-layer flexible suction cup (603).

2. The tomato picking robot based on UWB navigation according to claim 1, characterized in that, The harvesting robotic arm (5) includes an X-axis guide rail (501), an X-axis slider (502), a Y-axis guide rail (503), and a Y-axis slider (504). The X-axis slider (502) is slidably mounted on the X-axis guide rail (501), the Y-axis guide rail (503) is vertically fixedly connected to the X-axis slider (502), the Y-axis slider (504) is slidably mounted on the Y-axis guide rail (503), and the actuator assembly (6) is fixedly mounted on the front end of the Y-axis slider (504).

3. The tomato picking robot based on UWB navigation according to claim 1, characterized in that, A laser radar (8) is installed at the lower front end of the control cabinet (2). The scanning plane of the laser radar (8) is horizontally oriented towards the direction of travel of the tracked mobile chassis (1). A collection box (10) is horizontally installed at the upper rear end of the tracked mobile chassis (1). The collection box (10) is located inside the mounting frame (3). An interactive display screen (7) is provided on the top of the control cabinet (2).

4. The tomato picking robot based on UWB navigation according to claim 1, characterized in that, The RGB-D depth camera (602) is fixed to the top plane of the mounting bracket (601) and the optical axis of the lens is parallel to the axis of the multi-layer flexible suction cup (603). The multi-layer flexible suction cup (603) is a corrugated tubular structure with multi-level buffer creases. The output shaft of the rotary motor (604) is connected to the mounting base of the multi-layer flexible suction cup (603) through a gear transmission structure.

5. The tomato picking robot based on UWB navigation according to claim 3, characterized in that, The control system includes a central controller, which is electrically connected to the UWB positioning tag (9); The central controller is used to receive the distance data relative to the UWB positioning base station in the external environment measured by the UWB positioning tag (9), calculate the global coordinates of the robot using the positioning algorithm, and control the tracked mobile chassis (1) to navigate to the target position based on the preset electronic map stored in the internal storage. The central controller is also used to control the picking robot arm (5) and the actuator assembly (6) to complete the picking action according to the data fed back by the RGB-D depth camera (602).

6. The tomato picking robot based on UWB navigation according to claim 5, characterized in that, The central controller calculates the robot's global coordinates by: establishing a positioning calculation model based on distance intersection; establishing a distance constraint equation based on the measured distance and relative height difference between the UWB positioning tag (9) and each UWB positioning base station; solving the distance constraint equation using the least squares method; and smoothing the solution result using the Kalman filter algorithm to output the estimated real-time coordinates of the robot in the global coordinate system at the current moment.

7. The tomato picking robot based on UWB navigation according to claim 6, characterized in that, The central controller controls the navigation of the tracked mobile chassis (1) based on a preset electronic map by calculating the vertical distance from the estimated real-time coordinates to the navigation path line in the electronic map as the lateral deviation. Based on the PID control algorithm, the speed difference between the motors on the left and right sides of the tracked mobile chassis (1) is adjusted according to the lateral deviation to drive the robot to stay on the center line between the rows. When the difference between the estimated real-time coordinates and the coordinates of the target plant is less than the preset position matching threshold, the robot is controlled to stop.

8. The tomato picking robot based on UWB navigation according to claim 5, characterized in that, The central controller controls the picking robot arm (5) based on the data fed back by the RGB-D depth camera (602) in the following way: using a target detection model to identify ripe tomatoes in the image acquired by the RGB-D depth camera (602) and extract pixel coordinates, converting the pixel coordinates into three-dimensional coordinates in the camera coordinate system based on the depth value and the camera intrinsic parameter matrix, and using the hand-eye calibration matrix and the robot arm kinematics matrix to map the three-dimensional coordinates in the camera coordinate system into target coordinates in the robot arm base coordinate system, and generating multi-axis linkage interpolation commands.

9. A tomato-harvesting robot based on UWB navigation according to claim 5, characterized in that, The central controller controls the actuator assembly (6) to complete the picking action as follows: after the multi-layer flexible suction cup (603) contacts the fruit, the pneumatic driver (605) is turned on. When the negative pressure value is detected to be lower than the adsorption judgment threshold, the adsorption is determined to be successful. Then the rotary motor (604) is started to drive the fruit to rotate. At the same time, the load current of the rotary motor (604) is monitored. When the load current drops by more than the preset threshold, the fruit is determined to be separated and the rotation stops.

10. A tomato-harvesting robot based on UWB navigation according to claim 5, characterized in that, The central controller is also used to execute local obstacle avoidance logic: When the lidar (8) detects an obstacle in the virtual anti-collision monitoring area and the distance is less than the deceleration threshold, it controls the robot to decelerate. When the obstacle does not completely block the road, a local obstacle avoidance arc is generated based on the local grid map to control the tracked mobile chassis (1) to bypass it.

Images