Rapid citrus harvesting motion planning with pre-harvesting point and quad-tree

Aiming at the problems of low success rate and long planning time of harvesting robots in unstructured and complex environments, a harvesting motion planning algorithm based on pre-harvesting guide points (Pre -harvesting Guide Informed-RRT*, PGI-RRT*) is proposed. This algorithm is on the basis of progressively optimal informed RRT * algorithm. The third point between the starting point and the target point is selected as the pre-harvesting guide point. The four random trees are generated between the initial point, the target point and the guide point at the same time to quickly obtain the initial path. In the PGI-RRT* algorithm, P probability sampling is used to replace random sampling to improve the blindness of sampling. New nodes are generated by the dynamic step size. To do so can improve the speed and flexibility of the PGI-RRT* algorithm to explore the unknown space, and improve the convergence speed of the optimal path. 2D simulation experiments show that, compared with Informed-RRT*, the PGI-RRT* algorithm can reduce the time to find the initial path by about 75-86 %, and improve the success rate by 18-32 %. 3D simulation experiments and experiments in laboratory and field environments show that the PGI-RRT* algorithm proposed in this paper can significantly reduce the planning time and improve the planning success rate. Compared to the 9 algorithms added to the pre-harvesting guide points for double path planning in the OMPL, the PGI-RRT* algorithm can reduce the path planning time by 40-60 %, and improve the success rate of path planning to 95 %, and shorten the execution time of the manipulator by about 8 s.