Receding Horizon Trajectory Optimization Through Path Segments and Waypoints

Abstract Many robotic manipulation tasks require both unconstrained motion in free space and precise tracking of constrained paths. Seamlessly switching between these motion types while replanning trajectories online, for example, during human–robot interaction, is a challenging problem. We propose a novel receding-horizon trajectory optimization approach for robot manipulator arms that systematically combines free-space motions and predefined path segments, even for long task sequences that exceed the planning horizon. Depending on their reachability, receding horizon planning is possible by splitting the horizon into multiple segments around waypoints. Free-space motion or path-following constraints can be specified for each horizon segment. In addition, task-specific tolerances around waypoints and path segments can be considered. Simulation and experimental results show that the proposed receding-horizon trajectory planning approach achieves shorter trajectory durations than point-to-point MPC, better computation time and planning success than classical path-following in free-space motion, and significantly improved computational efficiency compared to offline planning methods while still delivering good trajectory quality. The online replanning capabilities of the presented planner are demonstrated in a robot drawing task with a KUKA LBR iiwa 14 R820, where the robot draws a path on a whiteboard that is moved during the drawing.