MPPI-DBaS: Safe Trajectory Optimization with Adaptive Exploration

Fanxin Wang \aref XJTLU, Yikun Cheng* \aref UIUC, Chuyuan Tao \aref UIUC · 2025

Read MPPI-DBaS: Safe Trajectory Optimization with Adaptive Exploration by Fanxin Wang \aref XJTLU, Yikun Cheng* \aref UIUC, Chuyuan Tao \aref UIUC (2025) in the Tower of Babel, a free online library of public-domain works preserved in clean, searchable text. No sign-up. The full text is served as a plain-text file at a predictable URL, where text is present: https://antarctic-labs.com/tower-of-babel/arxiv-2502-14387.txt.

Introduction

Self-driving planners sample many possible paths and pick the best, but safety near tight obstacles is hard to guarantee. Fanxin Wang and co-authors upgrade one popular sampler, MPPI, by folding in discrete barrier states. Barrier states track how close the vehicle comes to danger and keep the search honest about it.

The upgraded planner also adapts its exploration: wide open spaces need little searching, while tight gaps deserve more. In simulations threading a vehicle through closely placed obstacles, the method beat standard MPPI clearly. Success rates rose and tracking errors fell. The 2025 paper targets robots that must move both boldly and safely.

Catalog record

CREATORFanxin Wang \aref XJTLU, Yikun Cheng* \aref UIUC, Chuyuan Tao \aref UIUC
YEAR2025
COLLECTIONPAPERS
FORMATTXT
FILE SIZE30 KB
VERSION1.0
LICENSEhttp://creativecommons.org/licenses/by/4.0/
RIGHTSCC BY
SOURCEarXiv

Source: https://arxiv.org/abs/2502-14387

Browse the full library · About the Tower of Babel

Full text

The opening of the work, from the library's hosted text:

MPPI-DBaS: Safe Trajectory Optimization with Adaptive Exploration Fanxin Wang \aref XJTLU, Yikun Cheng* \aref UIUC, Chuyuan Tao \aref UIUC Abstract In trajectory optimization, Model Predictive Path Integral (MPPI) control is a sampling-based Model Predictive Control (MPC) framework that generates optimal inputs by efficiently simulating numerous trajectories. In practice, however, MPPI often struggles to guarantee safety assurance and balance efficient sampling in open spaces with the need for more extensive exploration under tight constraints. To address this challenge, we incorporate discrete barrier states (DBaS) into MPPI and propose a novel MPPI-DBaS algorithm that ensures system safety and enables adaptive exploration across diverse scenarios. We evaluate our method in simulation experiments where the vehicle navigates through closely placed obstacles. The results demonstrate that the proposed algorithm significantly outperforms standard MPPI, achieving a higher success rate and lower tracking errors. XJTLU]Department of Department of Mechatronics and Robotics, Xi’an Jiaotong-Liverpool University, P. R. China UIUC]Department of Mechanical Science and Engineering, University of Illinois at Urbana-Champaign, USA. keywords Stochastic Optimization, Constraint, MPPI, Barrier State 1 Introduction With the growing success of robotics applications, the field of robotic control has gained significant attention, leading to the development of more practical strategies for addressing increasingly complex challenges in both simulated and real-world environments. Among these strategies, trajectory optimization is critical, as it aims to determine control inputs that enable a system to achieve specified objectives—such as enhanced energy efficiency and collision avoidance—while adhering to stringent constraints. Model Predictive Control (MPC), a widely adopted optimization-based technique, achieves this by repeatedly solving constrained optimization problems. However, many MPC frameworks assume linear system dynamics and formulate these tasks as quadratic optimization problems. Such simplifications often fail to capture the intricate nonlinearities inherent in robotic systems, resulting in significant model deviations and suboptimal performance. In contrast, Model Predictive Path Integral (MPPI) control is a sampling-based nonlinear MPC approach that directly accommodates nonlinear system dynamics. By leveraging the parallel computational capabilities of modern CPUs and GPUs, MPPI efficiently samples thousands of trajectories around a nominal control sequence in real time. It then derives an optimal trajectory and corresponding control sequence by computing a weighted average of the sampled trajectories’ costs. A key advantage of MPPI over traditional MPC lies in its ability to handle cost functions without restrictive structural assumptions, allowing for non-quadratic and even discontinuous forms. Nevertheless, most MPC and MPPI implementations represent the robot as a point mass, ignoring its physical geometry, arising the inaccuracy in designing cost functions. Figure 1 : Demonstration of MPPI trajectory sampling. The slashed yellow curves cover the sampled trajectories. The red curve illustrates a sub-optimal trajectory resulting from extensive, high variance exploration. The green curve denotes a near-optimal, refined trajectory. Control Barrier Functions (CBFs) [ 1 ] offer a promising solution to this challenge by representing the robot’s collision boundaries as continuous and differentiable functions that can be integrated into the optimization process as hard constraints. Ensuring system safety via these constraints guarantees that if the system starts in a collision-free set, it will never leave that set under any feasible control sequence derived from the constrained optimization. Such properties have been widely applied in MPC-based methods [ 2 ] and learning-based approaches [ 3 ] . However, incorporating CBFs introduces additional constraints that increase the complexity of the underlying optimization problem, causing infeasibility and increasing the computational burden. Barrier states (BaS) [ 4 ] , a variant formulation of CBFs, address these issues by transforming safety constraints into system stabilization problems rather than embedding them directly into the optimization constraints. This shift simplifies the control scheme design and improves computational efficiency, facilitating more practical real-time implementations for complex robotic systems. In this work, we propose an MPPI-DBaS framework that ensures robot safety while enabling adaptive exploration through challenging environments, such as navigating narrow yet optimal paths to a destination as illustrated in Figure 1 . The remainder of this paper is organized as follows: Section 2 presents the detailed problem formulation and the necessary background on MPPI and DBaS; Section 3 describes our proposed algorithm; Section 4 illustrates the performance comparison with a standard MPPI approach using a ground vehicle example; and finally, Section 5 provides concluding remarks and discusses future research directions. 2 Preliminaries In this section, we formalize the optimal control problem and present a detailed overview of the MPPI and DBaS methodologies. 2.1 MPPI-based Control Problem with Constraints We consider the following discrete-time non-linear control system: x_{k+1}=f(x_{k},u_{k}) (1) where x_{k}\in\mathbb{R}^{n} represents the k th state. We use \mathbb{X}=(x_{0},x_{1},\ldots,x_{N-1})\in\mathbb{R}^{n\times N} to denote the resulting state over the time-horizon N , and \mathbb{U}=(u_{0},u_{1},\ldots,u_{N-1})\in\mathbb{R}^{m\times N} to denote the control sequence over the time-horizon N . In trajectory optimization, the objective is to identify a control sequence \mathbb{U} that transitions the system from an initial state x_{c} to a reference state x_{f} without incurring collisions. In contrast to gradient-based Model Predictive Control (MPC) approaches, Model Predictive Path Integral (MPPI) does not require the computation of derivatives, making it well-suited for highly nonlinear, non-convex objective functions and complex system dynamics [ 5 ] . The trajectory optimization problem in MPPI can be formulated as: \displaystyle\underset{\mathbb{U}}{\text{minimize}} \displaystyle J=\mathbb{E}[\phi(x_{N})+\sum_{k=0}^{N-1}(q(x_{k})+\frac{1}{2}v_{k}^{T}Rv_{k})], (2) \displaystyle\text{subject to} \displaystyle x_{k+1}=f(x_{k},v_{k}), \displaystyle x_{0}=x_{c}, \displaystyle G(u_{k})\leq 0, \displaystyle h(x_{k})\geq 0,\qquad k\in\{0,1,\cdots,N-1\} where \phi(x_{N}) is the terminal cost, q(x_{k}) and R are the state-dependent running cost and the positive definite control weighting matrix. v_{k}=u_{k}+\delta u_{k}\in\mathbb{R}^{m} represents the k th control input in trajectory sampling. We denote the \delta u_{k}\in\mathcal{N}(0,\Sigma_{u}) as the injected disturbance with zero mean and covariance \Sigma_{u} At each time step, MPPI generates M trajectories in parallel from system dynamics with the injected disturbance \delta u_{k} and then evaluates those trajectories based on their expected costs [ 6 ] . The cost-to-go of the state dependent cost of a trajectory is stated as: \displaystyle\tilde{S}(\tau)=\phi(x_{N})+\sum_{k=0}^{N-1}q(x_{k})+\gamma u_{k}^{T}\Sigma_{u}^{-1}v_{k} (3) where \gamma can be interpreted as the control-cost parameter. With \gamma=0 , the control sequence U is push close to zero with high energy conservation. With a large \gamma value, the resulting control sequence U is the distribution closer to the current planned control law, which keeps U near the distribution corresponding to \hat{U} . \Sigma_{u} is again the injected disturbance covariance, which dominates the exploration in parallel trajectory generation. At i^{t}h step the optimal control sequence \{u_{k}\}^{N-1}_{k=i} can be estimated as [ 5 ] : u_{k}^{*}=u_{k}+\frac{\sum_{m=1}^{M}\exp(-(1/\lambda)(\tilde{S}(\tau_{k,m})-\rho))\delta u_{k,m}}{\sum_{m=1}^{M}\exp(-(1/\lambda)(\tilde{S}(\tau_{k,m})-\rho))} (4) where \rho=\min_{k}[S_{k}] is the minimum sampled cost in Monte-Carlo approximation, and the parameter \lambda\in\mathbb{R}^{+} is known as the inverse temperature, which determines the selectivity of the weighted average of the trajectories. The control sequence is then smoothed using a Savitzky-Golay (SG) filter to reduce noise and improve the smoothness of the control inputs. Finally, the first control input u_{0} in the sequence is applied to the system, while the remaining control sequence of length N-1 is used for warm-starting the optimization at the next time-step. 2.2 Discrete Barrier States (DBaS) By definition, guaranteeing safety is equivalent to rendering the safe set controller invariant over the horizon. Definition 1 The set S\subset\mathbb{R}^{n} is said to be controlled invariant with respect to the finite horizon optimal control problem ( 2 ) under feedback policy U^{*}(x) , if \forall x_{0}\in S,x_{k}\in S\ \forall k\in[0,N] Enforcing controlled invariance can be achieved through various

Read the complete text