Given static and dynamic obstacles (red shaded regions), the drone samples candidate trajectories within the sampling region (gray shaded region). The sampled trajectories are projected away from obstacles along the direction indicated by the red arrows, e.g., from τ₁ to τ₁′. The projected trajectories are then clustered into distinct modes (green, orange, cyan, and magenta) to estimate the multimodal structure of the optimal trajectory distribution p(o = 1 | τ).
Summary
Crowded and dynamic environments require fast replanning; however, single-trajectory optimization lacks alternative paths, while parallel optimization often relies on predefined initial guidance. In this work, we identify multimodal optimal trajectory distributions without initial guidance by projecting sampled trajectories onto safe constraint sets, clustering them, and optimizing each cluster using MPPI. Simulation and hardware experiments demonstrate higher success rates and safe drone navigation in crowded 3D environments.
Method overview
The proposed framework to capture the multomodality of optimal trajectory distribution. Further details are provided in our paper.
We propose a method for safely replanning multimodal trajectories in complex 3D environments using projection-based trajectory clustering. Unlike prior works that rely on predefined guidance to generate multiple candidate trajectories, our method does not depend on any external planning structure. The proposed method consists of three stages:
Random control input sequences (i.e., trajectory) are sampled and projected onto the safe set, represented by affine constraints, to avoid obstacles.
The generated trajectories are clustered according to their projection directions, grouping trajectories that converge to the same local optimum. Without this clustering step, estimating the sampled trajectory probability density function (PDF) via forward KL divergence can lead to mode collapse.
Finally, we use the MPPI framework to estimate the local optima of the multimodal optimal trajectory distribution for each cluster.
Simulation and experiment results
Planning results for each sampling-based method.
As shown in the figure above, the map used for planning is shown in (a), and the comparison results are presented in (b)(e). (b) shows randomly sampled trajectories within the restricted control input range. (c) utilizes SVGD as the initial guidance. (d) employs the projection method for sampling collision-free trajectories. (b) through (d) fail to determine collision-free trajectories because they estimate an unimodal Gaussian distribution on a multi-modal distribution (mode collapse). However, our method (e) can generate multiple trajectories by utilizing clustering. (f) shows the grouping results of ours.
Simulation and hardware experiments demonstrate the effectiveness of the proposed method. The planner operates at 50 ms intervals and generates an average of 1.7 trajectories per planning cycle, consistently producing collision-free trajectories to the goal. Dynamic obstacles move randomly at speeds below 1 m/s and are perceived only within the sensing range, with a detection probability of 0.95. Their future motions are predicted using a constant-velocity model. Additional comparisons, quantitative evaluations, and details are provided in the paper.
BibTeX citation
@article{lim2026safe, title={Safe Multimodal Replanning via Projection-based Trajectory Clustering in Crowded Environments}, author={Lim, Yongjae and Jung, Seungwoo and Kim, Dabin and Lee, Dongjae and Kim, H Jin}, journal={IEEE Robotics and Automation Letters}, year={2026}, publisher={IEEE}}