Reachability-Oriented Pose Estimation and Efficient Path Planning for Tomato Harvesting Robots

Agriculture is currently transitioning toward higher intelligence and facility-based production, where harvesting robots play a crucial role in enhancing efficiency and ensuring standardized output. Addressing the challenges of inaccurate picking pose estimation and limited reachability in greenhouse environments, this paper proposes a reachable grasping pose estimation method based on Particle Swarm Optimization (PSO). First, initial poses are calculated via instance segmentation and keypoint extraction. Subsequently, a fitness function is constructed based on inverse kinematics, and the PSO algorithm is employed to iteratively search for optimal reachable poses. To further tackle planning difficulties in confined spaces, a two-stage path planning method based on cost maps is introduced. A series of performance metrics were designed to validate the proposed pose estimation and path planning methods through simulation experiments. In real-world field tests, the system achieved a harvesting success rate of 85%, significantly outperforming existing methods. The results demonstrate that the proposed approach substantially enhances the operational feasibility and success rate of tomato harvesting robots.