A sampling-based optimized algorithm for task-constrained motion planning

We consider a motion planning problem with task space constraints in a complex environment for redundant manipulators. For this problem, we propose a motion planning algorithm that combines kinematics control with rapidly exploring random sampling methods. Meanwhile, we introduce an optimization structure similar to dynamic programming into the algorithm. The proposed algorithm can generate an asymptotically optimized smooth path in joint space, which continuously satisfies task space constraints and avoids obstacles. We have confirmed that the proposed algorithm is probabilistically complete and asymptotically optimized. Finally, we conduct multiple experiments with path length and tracking error as optimization targets and the planning results reflect the optimization effect of the algorithm.