CSC-MPPI: A Novel Constrained MPPI Framework with DBSCAN for Reliable Obstacle Avoidance

Abstract: This paper proposes Constrained Sampling Cluster Model Predictive Path Integral (CSC-MPPI), a novel constrained formulation of MPPI designed to enhance trajectory optimization while enforcing strict constraints on system states and control inputs. Traditional MPPI, which relies on a probabilistic sampling process, often struggles with constraint satisfaction and generates suboptimal trajectories due to the weighted averaging of sampled trajectories. To address these limitations, the proposed framework integrates a primal-dual gradient-based approach and Density-Based Spatial Clustering of Applications with Noise (DBSCAN) to steer sampled input trajectories into feasible regions while mitigating risks associated with weighted averaging. First, to ensure that sampled trajectories remain within the feasible region, the primal-dual gradient method is applied to iteratively shift sampled inputs while enforcing state and control constraints. Then, DBSCAN groups the sampled trajectories, enabling the selection of representative control inputs within each cluster. Finally, among the representative control inputs, the one with the lowest cost is chosen as the optimal action. As a result, CSC-MPPI guarantees constraint satisfaction, improves trajectory selection, and enhances robustness in complex environments. Simulation and real-world experiments demonstrate that CSC-MPPI outperforms traditional MPPI in obstacle avoidance, achieving improved reliability and efficiency. The experimental videos are available at https://cscmppi.github.io

• The paper introduces CSC-MPPI, which applies a primal-dual gradient update to project all sampled trajectories into a feasible region, achieving a 0% collision rate for obstacle avoidance.
• The paper employs DBSCAN clustering to group feasible trajectories and effectively counter the shortcomings of weighted averaging in traditional MPPI methods.
• The paper demonstrates that CSC-MPPI produces shorter, efficient paths while maintaining real-time performance despite a modest increase in computational overhead.

Constrained Sampling Cluster MPPI (CSC-MPPI) with DBSCAN for Reliable Obstacle Avoidance

The paper introduces CSC-MPPI, a constrained extension of Model Predictive Path Integral (MPPI) control designed to robustly enforce hard state and control constraints in robot trajectory optimization. Unlike standard MPPI, which employs weighted averaging of sampled trajectories and is fundamentally limited to soft constraint handling, CSC-MPPI integrates a primal-dual gradient-based adjustment with a density-based clustering strategy (DBSCAN) to reliably generate feasible, efficient, and safe paths in environments where constraint violations are unacceptable.

Background and Limitations of Traditional MPPI

MPPI is a stochastic, sampling-based model predictive control scheme that achieves real-time trajectory planning by evaluating a set of future trajectory rollouts sampled from a distribution around a nominal control sequence. While effective in dynamic and uncertain settings, MPPI's trajectory averaging undermines its ability to enforce hard constraints on system states and control actions; constraint violations are common when samples concentrate in regions of high cost or when the sampling distribution lacks sufficient diversity. Empirical evidence in the paper demonstrates that, under conventional MPPI, even large increases in sample count can substantively reduce but not eliminate the collision rate with obstacles.

Prior research has proposed various mechanisms for improved constraint-handling within sampling-based MPC—e.g., output-space sampling, probabilistic hyperplane constraints, and stochastic barrier functions—however, these approaches still primarily rely on soft constraint formulations and cannot guarantee strict feasibility.

CSC-MPPI: Sequential Constraint Enforcement and Robust Trajectory Selection

CSC-MPPI modifies MPPI's sampling-selection pipeline in two principal ways:

1. Primal-Dual Gradient Adjustment for Feasibility: All sampled trajectories are iteratively projected into the feasible region defined by state and control constraints using a primal-dual gradient update. For each rollout, the Lagrangian is constructed with respect to all constraints; primal variables (control inputs) and dual variables (Lagrange multipliers) are updated via gradient descent and projection. This process continues until the Karush-Kuhn-Tucker (KKT) conditions are satisfied for each sample, guaranteeing feasibility of both the system's state evolution (e.g., obstacle avoidance) and control bounds.
2. DBSCAN-Based Trajectory Clustering:

Post constraint enforcement, the population of feasible trajectories is clustered using DBSCAN according to spatial proximity and cost value similarity. This serves several purposes: - Resilience to Weighted Averaging Artifacts: By grouping geometrically and cost-similar trajectories, then selecting representative candidates from each group, CSC-MPPI mitigates the risk that weighted averaging will produce a control input lying outside the feasible set (a known failure mode for naive MPPI, especially when multiple samples have similar costs). - De-noising: DBSCAN automatically rejects outliers and noise points that may represent unstable, high-cost, or inconsistent behaviors, thereby contributing to robust and stable control selection. - Adaptive Partitioning: Unlike parametric clustering (e.g., k-means), DBSCAN dynamically adapts to the sample distribution's structure per planning step, making it well-suited for the stochasticity inherent in MPPI.

The final control input is taken as the candidate with the lowest cost among the best representatives from each cluster—a procedure that, by construction, preserves feasibility guarantees.

Experimental Results: Numerical Claims and Performance Highlights

CSC-MPPI is evaluated in both simulated and real-world obstacle avoidance tasks with a differential-drive robot. Results highlight the core properties:

• Constraint Satisfaction Guarantees:

Across all tested environments, CSC-MPPI achieved a collision rate of 0%, in stark contrast to standard MPPI, which exhibited collision rates ranging from 80% (with 20 samples) to 30% (with 300 samples). Notably, CSC-MPPI retained zero collision rate even at low sample counts, emphasizing the primacy of explicit constraint enforcement.

• Path Efficiency:

CSC-MPPI produced consistently shorter trajectories (minimum 4.476 m at 300 samples) than standard MPPI (minimum 4.755 m at 50 samples and 4.848 m at 300 samples), indicating that constraint enforcement did not unduly restrict optimality in trajectory cost.

• Computation Time:

While CSC-MPPI incurs additional computational overhead (average per-step time about 8.9 ms compared to 1.7 ms for standard MPPI at high sample counts), this increase remains compatible with real-time operation due to parallelization and GPU acceleration. DBSCAN's contribution to computation time is minor and stable regardless of sample count.

• Robustness to Weighted Averaging:

In a controlled setting, omitting DBSCAN from CSC-MPPI resulted in only 80% of optimal averaged trajectories strictly satisfying constraints, despite all underlying rollouts being feasible. Incorporating DBSCAN restored this to 100%, demonstrating its necessity for strict feasibility at the controller output.

In real-world, CPU-only experiments, CSC-MPPI achieved a 0% collision rate in all 10 trials with computation bounded by a maximum of 38.89 ms per planning step.

Implications, Limitations, and Future Directions

CSC-MPPI represents a significant advance for safety-critical robotic systems where hard constraints on state and control cannot be violated, such as autonomous vehicles, mobile manipulators in human environments, or aerial robots in confined spaces. The key methodological contribution is showing that explicit primal-dual feasibility correction, coupled with density-based nonparametric clustering, overcomes the inherent weakness of sampling-based methods in safety assurance without sacrificing trajectory efficiency.

Practical implications include:

• Plug-and-play viability for systems requiring certified obstacle avoidance under strict actuation and state limits.
• Straightforward extension to GPU-parallelized or multi-agent scenarios, given the decomposability of both the primal-dual constraint enforcement and DBSCAN steps.
• Flexibility with respect to the dynamics model and cost structure, as well as real-time applicability with moderate computational resources.

Current limitations:

• Increased computation relative to soft-constrained MPPI may be non-negligible for high-frequency or power-constrained systems unless implementation is optimized for parallel hardware.
• In high-dimensional control (e.g., legged robots or manipulators), the effectiveness and scalability of the DBSCAN clustering step require further empirical evaluation, as density estimation in high dimensions can be challenging.
• The primal-dual update approach, while robust for simple bounds and norm-based constraints, might encounter slow convergence for nonconvex or tightly coupled constraints without further methodological enhancements.

Future research directions could focus on:

• Developing adaptive or learned approaches for sample allocation and step size selection within the primal-dual procedure.
• Integrating advanced density estimation techniques or more scalable clustering methods for high-dimensional state spaces.
• Extending the framework to robustify against model mismatch or partial observability, possibly by incorporating learned uncertainty models or sensor fusion.
• Empirical validation on more complex robotic systems, including those with hybrid dynamics or underactuation.

CSC-MPPI establishes a new paradigm for bridging the gap between safety-critical constraint enforcement and sample-based planning in real-time control, with potential impacts extending across robotic navigation, manipulation, and autonomous driving domains.