betaFeedback

WorksMathematics

Consensus-Based Optimization of Path-Integral Free Energy for Model Predictive Control

Abstract

We propose CBO-MPPI, a sampling-based predictive controller that applies consensus-based optimization (CBO) to the free energy underlying model predictive path integral control (MPPI). Under hard constraints, this objective rewards sampling neighborhoods containing feasible, low-cost trajectories under perturbation. Standard MPPI performs a local stochastic search by updating one nominal control sequence from Gaussian perturbations around it. In CBO-MPPI, we estimate the free energy at multiple nominal centers from local rollouts. These estimates define a shared consensus that guides drift and diffusion, and the final selected center supplies an MPPI weighted-mean output. For the associated mean-field model, we establish well-posedness and, under additional assumptions, convergence toward a free-energy minimizer. In simulations of obstacle navigation, pendulum swing-up, and planar-arm manipulation under equal rollout budgets, at high temperatures, CBO-MPPI attains lower free energy than MPPI and multi-start MPPI and maintains greater obstacle clearance than CBO-MPC.

Introduction

Model predictive control (MPC) repeatedly solves a finite-horizon optimization problem from the measured state. Its ability to incorporate nonlinear dynamics and constraints directly has led to widespread use in robotics, autonomous driving, and process control ([9]). The resulting optimization problems can be high-dimensional and nonconvex, with nonsmooth or discontinuous objectives arising from obstacle avoidance and hard constraints. Solving these problems efficiently within the available computation time is a central challenge in MPC.

Model predictive path integral control (MPPI) offers a sampling-based approach to the MPC subproblem ([11, 17, 18, 19]). It generates perturbed control sequences around a nominal sequence and updates that sequence by exponentially weighting the rollout costs. The update requires rollout costs without differentiating the dynamics or cost, and the rollouts can be evaluated in parallel. The objective underlying this update is a free energy defined through an exponential average of perturbed trajectory costs. Under hard constraints, a low free energy identifies a sampling neighborhood containing substantial probability mass on feasible, low-cost trajectories. With fixed Gaussian covariance, one MPPI update approximates a preconditioned gradient step on this free-energy objective ([6]). Because MPPI repeatedly updates one nominal sequence, it has a local search structure and may miss distant basins with lower free energy.

We propose CBO-MPPI to address this local-search limitation by evolving multiple MPPI sampling centers with consensus-based optimization (CBO). CBO is a gradient-free particle method for global optimization, driven by consensus and random exploration [14, 5, 8]. In CBO-MPPI, the particles are the centers, and the consensus favors those with lower free-energy estimates from local rollouts. All centers move toward this common consensus instead of taking independent MPPI steps. Since drift and diffusion depend on the distance to the consensus rather than on the local landscape, a center at a local minimum keeps moving as long as it differs from the consensus. At the final round, the center with the lowest estimated free energy supplies the control through the usual MPPI weighted mean.

Several related approaches also aim to improve exploration in sampling-based optimal control. Closest to our work, CBO has been applied directly to trajectory costs in finite-horizon optimal control problems [3, 16]. Other methods improve sampling efficiency by adapting where control sequences are sampled through adaptive importance sampling [1] and learned proposal distributions [15]. Variational and Stein MPC represent alternative solutions through multimodal control distributions [13, 12], while multi-modal MPPI uses maneuver-specific proposals [2]. SVG-MPPI guides MPPI toward a target mode by updating guide particles with surrogate gradients [10]. CBO-MPPI differs in how its centers interact: a free-energy-weighted consensus governs their gradient-free drift and diffusion.

In this paper, we make three contributions. First, CBO-MPPI applies CBO to the MPPI free energy over a population of sampling centers. Second, Gaussian smoothing yields an exact, unprojected mean-field model under compactness and bounded-measurability assumptions, even when hard constraints make the trajectory cost discontinuous. Additional assumptions yield finite-accuracy global convergence. Third, we compare CBO-MPPI with MPPI, multi-start MPPI, and CBO-MPC under equal rollout budgets on Navigation, Many Obstacles, Pendulum, and Planar Arm benchmarks. At high temperatures, CBO-MPPI has the lowest free energy among MPPI controllers and greater obstacle clearance than CBO-MPC; its MPPI free-energy advantage persists across tested settings.

Problem Formulation and Method

We define the constrained trajectory cost and MPPI free energy, then present MPPI and CBO-MPPI.

Constrained Trajectory Cost and Free Energy

We consider a discrete-time system with state and input dimensions nn and mm, respectively, and state-transition map f:Rn×Rm→Rnf : \mathbb{R}^{n} \times\mathbb{R}^{m} \to\mathbb{R}^{n}. At each control period, we fix the current state x∈Rnx \in\mathbb{R}^{n} and a prediction horizon of H≥1H \ge1 steps. A candidate control sequence U=(u0,…,uH−1)∈RDU = (u_0,\ldots,u_{H-1}) \in\mathbb{R}^{D}, with ut∈Rmu_t \in\mathbb{R}^{m} and D=mHD = mH, generates the predicted trajectory from x0=xx_0 = x through

xt+1=f(xt,ut),t=0,…,H−1.(1)x_{t+1} = f(x_t,u_t), \qquad t = 0,\ldots,H-1. \tag*{(1)}

For a stage cost ℓ:Rn×Rm→R\ell: \mathbb{R}^{n} \times\mathbb{R}^{m} \to\mathbb{R} and terminal cost g:Rn→Rg : \mathbb{R}^{n} \to\mathbb{R}, the trajectory cost along (1) is

Jx(U)=∑t=0H−1ℓ(xt,ut)+g(xH).(2)J_x(U) = \sum_{t=0}^{H-1} \ell(x_t,u_t) + g(x_H). \tag*{(2)}

For the control problem, we specify a state-constraint set Cx⊆Rn\mathcal{C}_x \subseteq\mathbb{R}^n and an actuator box Cu⊂Rm\mathcal{C}_u \subset\mathbb{R}^m. Let CuH:=∏t=0H−1Cu\mathcal{C}_u^H := \prod_{t=0}^{H-1}\mathcal{C}_u denote the HH-fold Cartesian product of the actuator set. The admissible input sequences satisfy both the input bounds and the state constraints along the predicted trajectory in (1):

C={U∈CuH:xt∈Cx,t=1,…,H}.\mathcal{C} = \{U \in\mathcal{C}_u^H : x_t \in\mathcal{C}_x,\quad t=1,\ldots,H\}.

We define the extended cost by Jˉx(U)=Jx(U)\bar{J}_x(U) = J_x(U) for U∈CU \in\mathcal{C} and Jˉx(U)=+∞\bar{J}_x(U) = +\infty otherwise. At each control period, MPC approximately minimizes the cost in (2) over U∈CU \in\mathcal{C} and applies the first input of the resulting sequence to the plant. We denote this first input by [U]0=u0[U]_0 = u_0.

For this fixed control problem, we make the following assumptions.

Assumption 1 (Admissible control set). The admissible control set C⊂RD\mathcal{C} \subset\mathbb{R}^D is compact and has positive Lebesgue measure.

Assumption 2 (Open-loop cost). For the fixed current state x∈Rnx \in\mathbb{R}^n, the open-loop cost JxJ_x in (2), restricted to C\mathcal{C}, is Borel measurable and bounded: there exist constants Jmin⁡,Jmax⁡∈RJ_{\min}, J_{\max} \in\mathbb{R} with Jmin⁡≤Jx(U)≤Jmax⁡J_{\min} \le J_x(U) \le J_{\max} for all U∈CU \in\mathcal{C}.

To define the free energy, fix a temperature τ>0\tau> 0 and a sampling covariance Σ∈RD×D\Sigma\in\mathbb{R}^{D \times D} with Σ≻0\Sigma\succ0. We use N(0,Σ)\mathcal{N}(0,\Sigma) for the Gaussian probability distribution and φΣ\varphi_\Sigma for its Lebesgue density. For a nominal sequence M∈RDM \in\mathbb{R}^D and ϵ∼N(0,Σ)\epsilon\sim\mathcal{N}(0,\Sigma), we define the free energy by

Fx(M):=−τlog⁡E[e−Jˉx(M+ϵ)/τ]=−τlog⁡∫CφΣ(U−M)e−Jx(U)/τ dU.(3)F_x(M) := -\tau\log\mathbb{E}\left[e^{-\bar{J}_x(M+\epsilon)/\tau}\right] = -\tau\log\int_{\mathcal{C}} \varphi_\Sigma(U-M)e^{-J_x(U)/\tau}\,\mathrm{d}U. \tag*{(3)}

Under Assumptions 1 and 2, the expectation is strictly positive and finite, and Gaussian convolution gives Fx∈C∞(RD)F_x \in C^\infty(\mathbb{R}^D). Appendix § 7.§ 1 proves these properties.

The free energy can be decomposed by conditioning on the perturbed sequence being admissible:

Fx(M)=−τlog⁡Pr⁡(M+ϵ∈C)−τlog⁡E[e−Jx(M+ϵ)/τ∣M+ϵ∈C].(4)F_x(M) = -\tau\log\Pr(M+\epsilon\in\mathcal{C}) - \tau\log\mathbb{E}\left[e^{-J_x(M+\epsilon)/\tau} \mid M+\epsilon\in\mathcal{C}\right]. \tag*{(4)}

The first term penalizes a small probability of drawing an admissible sequence; the second is a soft minimum of the costs conditional on admissibility. Thus a low value of Fx(M)F_x(M) identifies a sampling neighborhood around MM that contains sufficiently many admissible perturbations with low trajectory cost. The temperature τ\tau controls how strongly the exponential average favors lower costs and scales the feasibility penalty. This interpretation motivates optimizing free energy under hard constraints.

MPPI as Single-Center Search

MPPI draws LL samples Uj=M+ϵjU_j = M + \epsilon_j around a nominal sequence MM, where ϵj∼N(0,Σ)\epsilon_j \sim\mathcal{N}(0,\Sigma), and evaluates the rollout costs Sj=Jˉx(Uj)S_j = \bar{J}_x(U_j) along their predicted trajectories Williams et al., [18, 19]; Fazlyab et al. [6]. We use the batch average Z^=L−1∑j=1Le−Sj/τ\hat{Z} = L^{-1}\sum_{j=1}^{L}e^{-S_j/\tau} to normalize weights wjw_j, j=1,…,Lj=1,\ldots,L, that favor lower-cost samples. We update the nominal sequence to the weighted mean of the samples:

wj=e−Sj/τLZ^,M+=∑j=1LwjUj.(5)w_j = \frac{e^{-S_j/\tau}}{L\hat{Z}}, \qquad M^+ = \sum_{j=1}^{L}w_jU_j. \tag*{(5)}

The first input [M+]0[M^{+}]_0 supplies the current control action, and the remaining sequence provides an initial guess for the next control period. We define the exact-expectation MPPI update as the exponentially weighted mean

Φ(M):=∫RDUφΣ(U−M)e−Jˉx(U)/τ dU∫RDφΣ(U−M)e−Jˉx(U)/τ dU=M−τ−1Σ∇Fx(M).(6)\Phi(M) := \frac{\int_{\mathbb{R}^{D}} U\varphi_{\Sigma}(U-M)e^{-\bar{J}_x(U)/\tau}\,dU}{\int_{\mathbb{R}^{D}} \varphi_{\Sigma}(U-M)e^{-\bar{J}_x(U)/\tau}\,dU} = M-\tau^{-1}\Sigma\nabla F_x(M). \tag*{(6)}

The second equality is the fixed-covariance Gaussian MPPI gradient identity [6]. Under Assumptions 1 and 2, boundedness of JxJ_x on compact C\mathcal{C} provides an integrable bound for the weighted Gaussian-kernel derivatives, justifying differentiation under the integral. The finite-batch update (5) approximates this mean. Thus the expectation update Φ(M)\Phi(M) is a gradient step on FxF_x with preconditioner Σ/τ\Sigma/\tau. Each MPPI update thus estimates a descent direction from samples around a single center, so the search remains local. Appendix § 7.§ 1 states these identities and verifies their applicability, and Algorithm 2 gives the control-period flow used by MPPI.

CBO-MPPI

CBO-MPPI combines local MPPI rollout evaluation with CBO population search on free energy [14, 5]. We maintain NN nominal centers Mik∈RDM_i^k \in\mathbb{R}^{D}, i=1,…,Ni=1,\ldots,N, as particles and perform KK evaluation rounds, indexed by k=0,…,K−1k=0,\ldots,K-1. As in single-center MPPI, MM denotes a sampling center and UU a candidate input sequence. At round kk, each center uses the MPPI sampling construction: we draw LL sequences Ui,jk=Mik+ϵi,jkU_{i,j}^k=M_i^k+\epsilon_{i,j}^k, with ϵi,jk∼N(0,Σ)\epsilon_{i,j}^k\sim\mathcal{N}(0,\Sigma), and evaluate Si,jk=Jˉx(Ui,jk)S_{i,j}^k=\bar{J}_x(U_{i,j}^k) from the current state xx. The batch average used to normalize the MPPI weights in (5) also yields an estimate of the free energy in (3) at each center:

Z^ik=1L∑j=1Le−Si,jk/τ,F^ik=−τlog⁡Z^ik.(7)\widehat{Z}_i^k=\frac{1}{L}\sum_{j=1}^{L}e^{-S_{i,j}^k/\tau},\qquad\widehat{F}_i^k=-\tau\log\widehat{Z}_i^k. \tag*{(7)}

For a CBO inverse temperature α>0\alpha>0 and a round with at least one finite free-energy estimate, we define the center weights and consensus point

ωik=exp⁡(−αF^ik)∑r=1Nexp⁡(−αF^rk),M^αk:=∑i=1NωikMik.(8)\omega_i^k=\frac{\exp(-\alpha\widehat{F}_i^k)}{\sum_{r=1}^{N}\exp(-\alpha\widehat{F}_r^k)},\qquad\widehat{M}_{\alpha}^k:=\sum_{i=1}^{N}\omega_i^k M_i^k. \tag*{(8)}

References

  1. [1]Dylan M. Asmar, Ransalu Senanayake, Shawn Manuel, and Mykel J. Kochenderfer. Model Predictive Path Integral Strategies. In 2023 IEEE International Conference on Robotics and Automation (ICRA), pages 3182–3188, May 2023. doi: 10.1109/ICRA48891.2023.10160929.DOI
  2. [2]Alberto Bertipaglia, Dariu M. Gavrila, and Barys Shyrokau. Multi-Modal Model Predictive Path Integral Control for Collision Avoidance. arXiv preprint arXiv:2508.21364, August 2025. Accepted for oral presentation at the 29th IAVSD, Shanghai, China, August 18–22, 2025.arxiv.org/abs/2508.21364
  3. [3]Giacomo Borghi and Michael Herty. Model predictive control strategies using consensus-based optimization. Mathematical Control and Related Fields, 15(3):876–894, 2025. ISSN 2156-8472. doi: 10.3934/mcrf.2024054.DOI
  4. [4]José A. Carrillo, Young-Pil Choi, Claudia Totzeck, and Oliver Tse. An analytical framework for consensus-based global optimization method. Mathematical Models and Methods in Applied Sciences, 28(6):1037–1066, 2018. doi: 10.1142/S0218202518500276.DOI
  5. [5]José A. Carrillo, Shi Jin, Lei Li, and Yuhua Zhu. A consensus-based global optimization method for high dimensional machine learning problems. ESAIM: Control, Optimisation and Calculus of Variations, 27:S5, 2021. ISSN 1292-8119, 1262-3377. doi: 10.1051/cocv/2020046.DOI
  6. [6]Mahyar Fazlyab, Sina Sharifi, and Jiarui Wang. Model Predictive Path Integral Control as Preconditioned Gradient Descent. IEEE Control Systems Letters, 10:2089–2094, 2026. ISSN 2475-1456. doi: 10.1109/LCSYS.2026.3708745.DOI
  7. [7]Massimo Fornasier and Lukang Sun. A PDE framework of consensus-based optimization for objectives with multiple global minimizers. Communications in Partial Differential Equations, 50 (4):493–541, April 2025. ISSN 0360-5302. doi: 10.1080/03605302.2025.2459194.DOI
  8. [8]Massimo Fornasier, Timo Klock, and Konstantin Riedl. Consensus-Based Optimization Methods Converge Globally. SIAM Journal on Optimization, 34(3):2973–3004, September 2024. ISSN 1052-6234. doi: 10.1137/22M1527805.DOI
  9. [9]Lars Grüne and Jürgen Pannek. Nonlinear Model Predictive Control. In Lars Grüne and Jürgen Pannek, editors, Nonlinear Model Predictive Control: Theory and Algorithms, Communications and Control Engineering, pages 45–69. Springer International Publishing, Cham, 2017. ISBN 978-3-319-46024-6. doi: 10.1007/978-3-319-46024-6_3.DOI
  10. [10]Kohei Honda, Naoki Akai, Kosuke Suzuki, Mizuho Aoki, Hirotaka Hosogaya, Hiroyuki Okuda, and Tatsuya Suzuki. Stein Variational Guided Model Predictive Path Integral Control: Proposal and Experiments with Fast Maneuvering Vehicles. In 2024 IEEE International Conference on Robotics and Automation (ICRA), pages 7020–7026, May 2024. doi: 10.1109/ICRA57147.2024.10611021.
  11. [11]H. J. Kappen. Path integrals and symmetry breaking for optimal control theory. Journal of Statistical Mechanics: Theory and Experiment, 2005(11):P11011, November 2005. ISSN 1742-5468. doi: 10.1088/1742-5468/2005/11/P11011.DOI
  12. [12]Alexander Lambert, Fabio Ramos, Byron Boots, Dieter Fox, and Adam Fishman. Stein Variational Model Predictive Control. In Proceedings of the 2020 Conference on Robot Learning, volume 155 of Proceedings of Machine Learning Research, pages 1278–1297. PMLR, 2021. URL https://proceedings.mlr.press/v155/lambert21a.html.arxiv.org/abs/2011.07641
  13. [13]Masashi Okada and Tadahiro Taniguchi. Variational Inference MPC for Bayesian Model-based Reinforcement Learning. In Proceedings of the Conference on Robot Learning, volume 100 of Proceedings of Machine Learning Research, pages 258–272. PMLR, 2020. URL https://proceedings.mlr.press/v100/okada20a.html.arxiv.org/abs/1907.04202
  14. [14]René Pinnau, Claudia Totzeck, Oliver Tse, and Stephan Martin. A consensus-based model for global optimization and its mean-field limit. Mathematical Models and Methods in Applied Sciences, 27 (01):183–204, January 2017. ISSN 0218-2025. doi: 10.1142/S0218202517400061.DOI
  15. [15]Jacob Sacks and Byron Boots. Learning Sampling Distributions for Model Predictive Control. In Proceedings of The 6th Conference on Robot Learning, volume 205 of Proceedings of Machine Learning Research, pages 1733–1742. PMLR, 2023. URL https://proceedings.mlr.press/v205/sacks23a.html.arxiv.org/abs/2212.02587
  16. [16]Xudong Sun, Armand Jordana, Massimo Fornasier, Jalal Etesami, and Majid Khadiv. Consensus-based optimization (CBO): Towards Global Optimality in Robotics. In Robotics: Science and Systems XXII. Robotics: Science and Systems Foundation, July 2026. doi: 10.15607/RSS.2026.XXII.161.DOI
  17. [17]Evangelos Theodorou, Jonas Buchli, and Stefan Schaal. A Generalized Path Integral Control Approach to Reinforcement Learning. Journal of Machine Learning Research, 11(104):3137–3181, 2010. ISSN 1533-7928.DOI
  18. [18]Grady Williams, Andrew Aldrich, and Evangelos A. Theodorou. Model Predictive Path Integral Control: From Theory to Parallel Computation. Journal of Guidance, Control, and Dynamics, 40 (2):344–357, 2017. doi: 10.2514/1.G001921.DOI
  19. [19]Grady Williams, Paul Drews, Brian Goldfain, James M. Rehg, and Evangelos A. Theodorou. Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving. IEEE Transactions on Robotics, 34(6):1603–1622, 2018. ISSN 1941-0468. doi: 10.1109/TRO.2018.2865891.DOI

Paper details

vlnc:W_jus5r8zb
Version
v1 · latest
Published
Fresh — added to the Hub within the last 30 days
Added
License
CC BY 4.0
Usage

views · 0

downloads · 0

A view counts once per browser per day, a download each time the PDF is requested.

  • Mathematics

Generative AI tools

View statement
Looking for related papers…
Contents