D2MFusion: An End-To-End Differentiable Trajectory Optimizer for Safe Reactive Navigation

Abstract

Data-driven methods provide effective solutions for robot trajectory generation in dynamic environments. Many physical constraints exist in the real world, and understanding these constraints to generate feasible trajectories for kinematics or dynamics is highly demanding regarding the data quantity. Due to the black box, it is also challenging to ensure the safety of the trajectories planned by data-driven models. In this paper, we propose an end-to-end model (D2MFusion) that fuses data-driven components and a model-based optimizer. D2MFusion uses a differentiable optimization layer (dLQR) that forms a backpropagation loop with a perception network. With the input BEV image, the perception network outputs the environmental feature vector to adjust the optimizer parameters to adapt to the dynamic environment. We train this fusion planner to imitate expert trajectories on a real self-driving dataset and demonstrate the planner’s explainability, data efficiency, and safe reactivity through closed-loop simulations. We also conduct experiments on a real quadrupedal robot (Unitree Go2) in three different scenarios to demonstrate the ability of our method to navigate in dynamic environments.