*Article* **Motion Planning of the Citrus-Picking Manipulator Based on the TO-RRT Algorithm**

**Cheng Liu 1, Qingchun Feng 2,\*, Zuoliang Tang 1, Xiangyu Wang 3, Jinping Geng <sup>1</sup> and Lijia Xu 1,\***


**Abstract:** The working environment of a picking robot is complex, and the motion-planning algorithm of the picking manipulator will directly affect the obstacle avoidance effect and picking efficiency of the manipulator. In this study, a time-optimal rapidly-exploring random tree (TO-RRT) algorithm is proposed. First, this algorithm controls the target offset probability of the random tree through the potential field and introduces a node-first search strategy to make the random tree quickly escape from the repulsive potential field. Second, an attractive step size and a "step-size dichotomy" are proposed to improve the directional search ability of the random tree outside the repulsive potential field and solve the problem of an excessively large step size in extreme cases. Finally, a regression superposition algorithm is used to enhance the ability of the random tree to explore unknown space in the repulsive potential field. In this paper, independent experiments were carried out in MATLAB, MoveIt!, and real environments. The path-planning speed was increased by 99.73%, the path length was decreased by 17.88%, and the number of collision detections was reduced by 99.08%. The TO-RRT algorithm can be used to provide key technical support for the subsequent design of picking robots.

**Keywords:** picking manipulator; motion planning; TO-RRT; step-size dichotomy; regression superposition
