Incrementally Stochastic and Accelerated Gradient Information mixed Optimization for Manipulator Motion Planning

This paper introduces a novel motion planning algorithm, incrementally stochastic and accelerated gradient information mixed optimization (iSAGO), for robotic manipulators in a narrow workspace. Primarily, we propose the overall scheme of iSAGO integrating the accelerated and stochastic gradient information for an efficient constrained optimization based on the penalty method. In the stochastic part, we generate the adaptive stochastic moment via the random selection of collision checkboxes, interval time-series, and penalty factor based on the adaptive momentum (Adam) method to solve the body-obstacle stuck case. Due to the slow convergence of the stochastic part, we integrate the accelerated gradient descent (AGD) to improve the planning efficiency. Moreover, we adopt the Bayesian tree inference (BTI) to transform the whole trajectory optimization (SAGO) into an incremental sub-trajectory optimization (iSAGO), which improves the computation efficiency and success rate. Finally, we tune the key parameters and benchmark iSAGO against other planners (CHOMP, GPMP2, TrajOpt, STOMP, and RRT-Connect) on LBR-iiwa in a bookshelf and AUBO-i5 in a storage shelf. The result shows the highest success rate and moderate solving efficiency of iSAGO.