Coupled Mobile Manipulation via Trajectory Optimization with Free Space Decomposition

This paper presents a real-time method for whole-body trajectory optimization of mobile manipulators in simplified dynamic and unstructured environments. Current trajectory optimization methods typically use decoupling of the mobile base and the robotic arm, which reduces flexibility in motion, does not scale to unstructured environments, and does not consider the future evolution of the environment, which is crucial to avoid dynamic obstacles. Given a goal configuration, such as waypoints generated by a global path planner, we formulate a receding horizon trajectory optimization minimizing the distance-to-target while avoiding collisions with static and dynamic obstacles. The presented method unifies the control of a robotic arm and a non-holonomic base to allow coupled trajectory planning. For collision avoidance, we propose to compute three convex regions englobing the robot's major body parts (i.e., base, shoulder-link and wrist-link) and thus reducing and limiting the number of inequality constraints, regardless of the number of obstacles in the environment. Moreover, our approach incorporates predicted trajectory information to smoothly, and in advance, avoid dynamic obstacles. The presented results show that trajectory optimization for the coupled system can reduce the total execution time by 48% and that applying the convex region generation for individual links allows keeping the computational costs low, even for complex scenarios, enabling onboard implementation.