A Fast Two-Stage Approach for Multi-Goal Path Planning in a Fruit Tree

We consider the problem of planning the motion of a drone equipped with a robotic arm, tasked with bringing its end-effector up to many (150+) targets in a fruit tree; to inspect every piece of fruit, for example. The task is complicated by the intersection of a version of Neighborhood TSP (to find an optimal order and a pose to visit every target), and a robotic motion-planning problem through a planning space that features numerous cavities and narrow passages that confuse common techniques. In this contribution, we present a framework that decomposes the problem into two stages: planning approach paths for every target, and quickly planning between the start points of those approach paths. Then, we compare our approach by simulation to a more straightforward method based on multi- query planning, showing that our approach outperforms it in both time and solution cost.