RAMPAGE: Toward Whole-Body, Real-Time, and Agile Motion Planning in Unknown Cluttered Environments for Mobile Manipulators

It is an open problem for a mobile manipulator (MM) with a differential mobile base (MB) to quickly detect surrounding obstacles using onboard sensors and find whole-body collision-free trajectories under kinematic constraints in unknown cluttered environments. To this end, we propose a framework integrating perception, planning, and control, named RAMPAGE. First, we utilize a layered map including an occupancy grid map and Euclidean signed distance field to realize one-shot collision checking between obstacles and the MM, whose body is approximated by sphere decomposition. Second, based on the guidance of different topological paths, a novel hierarchical trajectory searching method is developed to find an initial whole-body kinodynamic trajectory for the MM. Finally, we derive and implement a whole-body trajectory optimization method in an augmented Lagrangian differential dynamic programming form, yielding an optimal feedback-feedforward controller for free during the optimization. Extensive benchmark comparisons and experiments demonstrate that the proposed framework can find near-optimal whole-body collision-free trajectories in real time with a high success rate. The high-frequency controller accurately tracks them at an average speed of 1.86 m/s among unknown dynamic obstacles with a maximum speed of 2 m/s in simulation. Pick-and-place experimental results further verify our method's effectiveness and efficiency in the real world.