Multimodal Fusion-Guided Diffusion Policy for Motion Planning in Rugged and Obstacle-Dense Environments

Abstract

Motion planning in unstructured environments remains a challenging task, particularly in scenarios with dense obstacles and discontinuous freespace, due to the need to ensure both safety and real-time performance for robots. To address these challenges, this paper proposes a multimodal fusion-guided diffusion policy framework, abbreviated as M-DP, synergistically guided by images, LiDAR points, and goal targets. A multimodal early-fusion mechanism is designed to combine visual and LiDAR data, leveraging the complementary nature of sensor observations to enhance obstacle perception. The fused feature vectors are utilized to guide the diffusion policy to generate multiple trajectories, and the Denoising Diffusion Implicit Model (DDIM) is employed for inference to improve real-time performance. Semantic and geometric constraints are incorporated to determine the optimal trajectory, enabling the selection of collision-free paths that balance safety, goal reaching, and bumpiness. Additionally, dynamic constraints are introduced to ensure the safety of robots in rugged and obstacle-dense environments. Real-world experimental evaluations demonstrate the safety and effectiveness of the framework compared to baseline methods, with ablation studies validating the contributions of key components. Codes and our self-collected dataset are available on https://github.com/xhy1599/M-DP.