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 n n n and m m m , respectively, and state-transition map f : R n × R m → R n f : \mathbb{R}^{n} \times\mathbb{R}^{m} \to\mathbb{R}^{n} f : R n × R m → R n . At each control period, we fix the current state x ∈ R n x \in\mathbb{R}^{n} x ∈ R n and a prediction horizon of H ≥ 1 H \ge1 H ≥ 1 steps. A candidate control sequence U = ( u 0 , … , u H − 1 ) ∈ R D U = (u_0,\ldots,u_{H-1}) \in\mathbb{R}^{D} U = ( u 0 , … , u H − 1 ) ∈ R D , with u t ∈ R m u_t \in\mathbb{R}^{m} u t ∈ R m and D = m H D = mH D = m H , generates the predicted trajectory from x 0 = x x_0 = x x 0 = x through
x t + 1 = f ( x t , u t ) , t = 0 , … , H − 1. (1) x_{t+1} = f(x_t,u_t), \qquad t = 0,\ldots,H-1. \tag*{(1)} x t + 1 = f ( x t , u t ) , t = 0 , … , H − 1. (1)
For a stage cost ℓ : R n × R m → R \ell: \mathbb{R}^{n} \times\mathbb{R}^{m} \to\mathbb{R} ℓ : R n × R m → R and terminal cost g : R n → R g : \mathbb{R}^{n} \to\mathbb{R} g : R n → R , the trajectory cost along (1) is
J x ( U ) = ∑ t = 0 H − 1 ℓ ( x t , u t ) + g ( x H ) . (2) J_x(U) = \sum_{t=0}^{H-1} \ell(x_t,u_t) + g(x_H). \tag*{(2)} J x ( U ) = t = 0 ∑ H − 1 ℓ ( x t , u t ) + g ( x H ) . (2)
For the control problem, we specify a state-constraint set C x ⊆ R n \mathcal{C}_x \subseteq\mathbb{R}^n C x ⊆ R n and an actuator box C u ⊂ R m \mathcal{C}_u \subset\mathbb{R}^m C u ⊂ R m . Let C u H : = ∏ t = 0 H − 1 C u \mathcal{C}_u^H := \prod_{t=0}^{H-1}\mathcal{C}_u C u H := ∏ t = 0 H − 1 C u denote the H H H -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 ∈ C u H : x t ∈ C x , t = 1 , … , H } . \mathcal{C} = \{U \in\mathcal{C}_u^H : x_t \in\mathcal{C}_x,\quad t=1,\ldots,H\}. C = { U ∈ C u H : x t ∈ C x , t = 1 , … , H } .
We define the extended cost by J ˉ x ( U ) = J x ( U ) \bar{J}_x(U) = J_x(U) J ˉ x ( U ) = J x ( U ) for U ∈ C U \in\mathcal{C} U ∈ C and J ˉ x ( U ) = + ∞ \bar{J}_x(U) = +\infty J ˉ x ( U ) = + ∞ otherwise. At each control period, MPC approximately minimizes the cost in (2) over U ∈ C U \in\mathcal{C} U ∈ C and applies the first input of the resulting sequence to the plant. We denote this first input by [ U ] 0 = u 0 [U]_0 = u_0 [ U ] 0 = u 0 .
For this fixed control problem, we make the following assumptions.
Assumption 1 (Admissible control set). The admissible control set C ⊂ R D \mathcal{C} \subset\mathbb{R}^D C ⊂ R D is compact and has positive Lebesgue measure.
Assumption 2 (Open-loop cost). For the fixed current state x ∈ R n x \in\mathbb{R}^n x ∈ R n , the open-loop cost J x J_x J x in (2) , restricted to C \mathcal{C} C , is Borel measurable and bounded: there exist constants J min , J max ∈ R J_{\min}, J_{\max} \in\mathbb{R} J m i n , J m a x ∈ R with J min ≤ J x ( U ) ≤ J max J_{\min} \le J_x(U) \le J_{\max} J m i n ≤ J x ( U ) ≤ J m a x for all U ∈ C U \in\mathcal{C} U ∈ C .
To define the free energy, fix a temperature τ > 0 \tau> 0 τ > 0 and a sampling covariance Σ ∈ R D × D \Sigma\in\mathbb{R}^{D \times D} Σ ∈ R D × D with Σ ≻ 0 \Sigma\succ0 Σ ≻ 0 . We use N ( 0 , Σ ) \mathcal{N}(0,\Sigma) N ( 0 , Σ ) for the Gaussian probability distribution and φ Σ \varphi_\Sigma φ Σ for its Lebesgue density. For a nominal sequence M ∈ R D M \in\mathbb{R}^D M ∈ R D and ϵ ∼ N ( 0 , Σ ) \epsilon\sim\mathcal{N}(0,\Sigma) ϵ ∼ N ( 0 , Σ ) , we define the free energy by
F x ( M ) : = − τ log E [ e − J ˉ x ( M + ϵ ) / τ ] = − τ log ∫ C φ Σ ( U − M ) e − J x ( U ) / τ d U . (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)} F x ( M ) := − τ log E [ e − J ˉ x ( M + ϵ ) / τ ] = − τ log ∫ C φ Σ ( U − M ) e − J x ( U ) / τ d U . (3)
Under Assumptions 1 and 2, the expectation is strictly positive and finite, and Gaussian convolution gives F x ∈ C ∞ ( R D ) F_x \in C^\infty(\mathbb{R}^D) F x ∈ C ∞ ( R D ) . Appendix § 7 .§ 1 proves these properties.
The free energy can be decomposed by conditioning on the perturbed sequence being admissible:
F x ( M ) = − τ log Pr ( M + ϵ ∈ C ) − τ log E [ e − J x ( 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)} F x ( M ) = − τ log Pr ( M + ϵ ∈ C ) − τ log E [ e − J x ( M + ϵ ) / τ ∣ M + ϵ ∈ C ] . (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 F x ( M ) F_x(M) F x ( M ) identifies a sampling neighborhood around M M M 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 L L L samples U j = M + ϵ j U_j = M + \epsilon_j U j = M + ϵ j around a nominal sequence M M M , where ϵ j ∼ N ( 0 , Σ ) \epsilon_j \sim\mathcal{N}(0,\Sigma) ϵ j ∼ N ( 0 , Σ ) , and evaluates the rollout costs S j = J ˉ x ( U j ) S_j = \bar{J}_x(U_j) S j = 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 = 1 L e − S j / τ \hat{Z} = L^{-1}\sum_{j=1}^{L}e^{-S_j/\tau} Z ^ = L − 1 ∑ j = 1 L e − S j / τ to normalize weights w j w_j w j , j = 1 , … , L j=1,\ldots,L j = 1 , … , L , that favor lower-cost samples. We update the nominal sequence to the weighted mean of the samples:
w j = e − S j / τ L Z ^ , M + = ∑ j = 1 L w j U j . (5) w_j = \frac{e^{-S_j/\tau}}{L\hat{Z}}, \qquad M^+ = \sum_{j=1}^{L}w_jU_j. \tag*{(5)} w j = L Z ^ e − S j / τ , M + = j = 1 ∑ L w j U j . (5)
The first input [ M + ] 0 [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 ) : = ∫ R D U φ Σ ( U − M ) e − J ˉ x ( U ) / τ d U ∫ R D φ Σ ( U − M ) e − J ˉ x ( U ) / τ d U = M − τ − 1 Σ ∇ F x ( 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)} Φ ( M ) := ∫ R D φ Σ ( U − M ) e − J ˉ x ( U ) / τ d U ∫ R D U φ Σ ( U − M ) e − J ˉ x ( U ) / τ d U = M − τ − 1 Σ∇ F x ( M ) . (6)
The second equality is the fixed-covariance Gaussian MPPI gradient identity [6 ] . Under Assumptions 1 and 2, boundedness of J x J_x J x on compact C \mathcal{C} 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) Φ ( M ) is a gradient step on F x F_x F 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 N N N nominal centers M i k ∈ R D M_i^k \in\mathbb{R}^{D} M i k ∈ R D , i = 1 , … , N i=1,\ldots,N i = 1 , … , N , as particles and perform K K K evaluation rounds, indexed by k = 0 , … , K − 1 k=0,\ldots,K-1 k = 0 , … , K − 1 . As in single-center MPPI, M M M denotes a sampling center and U U U a candidate input sequence. At round k k k , each center uses the MPPI sampling construction: we draw L L L sequences U i , j k = M i k + ϵ i , j k U_{i,j}^k=M_i^k+\epsilon_{i,j}^k U i , j k = M i k + ϵ i , j k , with ϵ i , j k ∼ N ( 0 , Σ ) \epsilon_{i,j}^k\sim\mathcal{N}(0,\Sigma) ϵ i , j k ∼ N ( 0 , Σ ) , and evaluate S i , j k = J ˉ x ( U i , j k ) S_{i,j}^k=\bar{J}_x(U_{i,j}^k) S i , j k = J ˉ x ( U i , j k ) from the current state x x x . 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 ^ i k = 1 L ∑ j = 1 L e − S i , j k / τ , F ^ i k = − τ log Z ^ i k . (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)} Z i k = L 1 j = 1 ∑ L e − S i , j k / τ , F i k = − τ log Z i k . (7)
For a CBO inverse temperature α > 0 \alpha>0 α > 0 and a round with at least one finite free-energy estimate, we define the center weights and consensus point
ω i k = exp ( − α F ^ i k ) ∑ r = 1 N exp ( − α F ^ r k ) , M ^ α k : = ∑ i = 1 N ω i k M i k . (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)} ω i k = ∑ r = 1 N exp ( − α F r k ) exp ( − α F i k ) , M α k := i = 1 ∑ N ω i k M i k . (8)
After each of the first K − 1 K-1 K − 1 evaluation rounds, the shared point M ^ α k \widehat{M}_{\alpha}^k M α k drives the consensus drift and diffusion of all centers. The Euler–Maruyama scheme gives
M i k + 1 = M i k − λ Δ s ( M i k − M ^ α k ) + σ c b o ∥ M i k − M ^ α k ∥ ξ i k Δ s , ξ i k ∼ N ( 0 , I D ) . (9) M_i^{k+1}=M_i^k-\lambda\Delta s\left(M_i^k-\widehat{M}_{\alpha}^k\right)+\sigma_{\mathrm{cbo}}\left\lVert M_i^k-\widehat{M}_{\alpha}^k\right\rVert\xi_i^k\sqrt{\Delta s},\qquad\xi_i^k\sim\mathcal{N}(0,I_D). \tag*{(9)} M i k + 1 = M i k − λ Δ s ( M i k − M α k ) + σ cbo M i k − M α k ξ i k Δ s , ξ i k ∼ N ( 0 , I D ) . (9)
Here λ > 0 \lambda>0 λ > 0 is the drift strength, σ c b o > 0 \sigma_{\mathrm{cbo}}>0 σ cbo > 0 the diffusion strength, and Δ s > 0 \Delta s>0 Δ s > 0 the optimization step size; ∥ ⋅ ∥ \lVert\cdot\rVert ∥ ⋅ ∥ denotes the Euclidean norm and I D I_D I D the D D D -dimensional identity matrix. The independent Gaussian vectors ξ i k \xi_i^k ξ i k produce diffusion proportional to each center’s distance from consensus. At the final round, we select i ⋆ ∈ arg min i F ^ i K − 1 i^\star\in\arg\min_i\widehat{F}_i^{K-1} i ⋆ ∈ arg min i F i K − 1 and reuse its batch for the MPPI weights and output
w j ⋆ = e − S i ⋆ , j K − 1 / τ L Z ^ i ⋆ K − 1 , U o u t = ∑ j = 1 L w j ⋆ U i ⋆ , j K − 1 , u a p p l y = [ U o u t ] 0 . (10) w_j^\star=\frac{e^{-S_{i^\star,j}^{K-1}/\tau}}{L\widehat{Z}_{i^\star}^{K-1}},\qquad U_{\mathrm{out}}=\sum_{j=1}^{L}w_j^\star U_{i^\star,j}^{K-1},\qquad u_{\mathrm{apply}}=[U_{\mathrm{out}}]_0. \tag*{(10)} w j ⋆ = L Z i ⋆ K − 1 e − S i ⋆ , j K − 1 / τ , U out = j = 1 ∑ L w j ⋆ U i ⋆ , j K − 1 , u apply = [ U out ] 0 . (10)
We warm-start the next MPC period by shifting the final evaluated centers and adding independent smooth jitter. The operator Shift removes the first input, shifts the remaining H − 1 H-1 H − 1 inputs one step to the left, and appends a terminal input; the numerical implementation appends zero. Viewing each center as an H × m H \times m H × m array, we use a basis B j i t ∈ R H × R j i t B_{\mathrm{jit}} \in\mathbb{R}^{H \times R_{\mathrm{jit}}} B jit ∈ R H × R jit of R j i t R_{\mathrm{jit}} R jit low-frequency cosine modes along the horizon and set ζ i = σ j i t B j i t A i \zeta_i = \sigma_{\mathrm{jit}} B_{\mathrm{jit}} A_i ζ i = σ jit B jit A i . Here σ j i t \sigma_{\mathrm{jit}} σ jit sets the jitter amplitude, and each A i ∈ R R j i t × m A_i \in\mathbb{R}^{R_{\mathrm{jit}} \times m} A i ∈ R R jit × m has independent standard Gaussian entries. Algorithm 1 gives the control-period procedure, and Appendix C.§ 1 specifies the shift and jitter construction shared by the population methods.
Algorithm 1: CBO-MPPI: population search on MPPI free energy
Require: Current state x x x , horizon H H H , initialized or warm-started centers M i 0 ∈ R m H M_i^0 \in\mathbb{R}^{mH} M i 0 ∈ R m H , i = 1 , … , N i=1,\ldots,N i = 1 , … , N ; local rollouts per center L L L , evaluation rounds K K K ; τ , α > 0 \tau,\alpha> 0 τ , α > 0 , Σ ≻ 0 \Sigma\succ0 Σ ≻ 0 ; λ , σ c b o , Δ s > 0 \lambda,\sigma_{\mathrm{cbo}},\Delta_s > 0 λ , σ cbo , Δ s > 0 ; σ j i t \sigma_{\mathrm{jit}} σ jit and B j i t B_{\mathrm{jit}} B jit
Ensure: Applied input u a p p l y u_{\mathrm{apply}} u apply and next-period centers M i 0 , n e x t M_i^{0,\mathrm{next}} M i 0 , next
for k = 0 , … , K − 1 k=0,\ldots,K-1 k = 0 , … , K − 1 do
for all centers i = 1 , … , N i=1,\ldots,N i = 1 , … , N in parallel do
Draw ϵ i , j k ∼ N ( 0 , Σ ) \epsilon_{i,j}^k \sim\mathcal{N}(0,\Sigma) ϵ i , j k ∼ N ( 0 , Σ ) and set U i , j k = M i k + ϵ i , j k U_{i,j}^k=M_i^k+\epsilon_{i,j}^k U i , j k = M i k + ϵ i , j k , j = 1 , … , L j=1,\ldots,L j = 1 , … , L
Roll out from x x x and evaluate S i , j k = J ˉ x ( U i , j k ) S_{i,j}^k=\bar{J}_x(U_{i,j}^k) S i , j k = J ˉ x ( U i , j k ) for all j j j in parallel
Compute F ^ i k \widehat{F}_i^k F i k by (7)
end for
Compute the consensus M ^ α k \widehat{M}_{\alpha}^k M α k by (8)
if k < K − 1 k<K-1 k < K − 1 then
Draw ξ i k ∼ N ( 0 , I D ) \xi_i^k \sim\mathcal{N}(0,I_D) ξ i k ∼ N ( 0 , I D ) and update every center by (9)
end if
end for
Select i ⋆ ∈ arg min i F ^ i K − 1 i^\star\in\arg\min_i \widehat{F}_i^{K-1} i ⋆ ∈ arg min i F i K − 1
Reuse the selected batch to compute U o u t U_{\mathrm{out}} U out by (10) ; apply u a p p l y = [ U o u t ] 0 u_{\mathrm{apply}}=[U_{\mathrm{out}}]_0 u apply = [ U out ] 0
for all i = 1 , … , N i=1,\ldots,N i = 1 , … , N do
Draw A i A_i A i with independent N ( 0 , 1 ) \mathcal{N}(0,1) N ( 0 , 1 ) entries and set ζ i = σ j i t B j i t A i \zeta_i=\sigma_{\mathrm{jit}}B_{\mathrm{jit}}A_i ζ i = σ jit B jit A i
Set M i 0 , n e x t = Shift ( M i K − 1 ) + ζ i M_i^{0,\mathrm{next}}=\operatorname{Shift}(M_i^{K-1})+\zeta_i M i 0 , next = Shift ( M i K − 1 ) + ζ i
end for
Remark 3 (Temperature parameters) For a round with at least one finite free-energy estimate, suppressing the index k k k , the center weights in (8) satisfy ω i ∝ e − α F ^ i = Z ^ i α τ \omega_i \propto e^{-\alpha\widehat{F}_i}= \widehat{Z}_i^{\alpha\tau} ω i ∝ e − α F i = Z i α τ . Thus τ \tau τ controls exponential weighting of rollouts within each batch, while α τ \alpha\tau α τ is the exponent applied to the batch averages. Since Z ^ i \widehat{Z}_i Z i itself depends on τ \tau τ , fixing α τ \alpha\tau α τ does not make the weights independent of τ \tau τ .
Remark 4 (Input projection and zero-weight batches) For numerical rollout evaluation, each input sequence is projected onto C u H C_u^H C u H before simulating its trajectory. Denoting this projection by Π C u H \Pi_{C_u^H} Π C u H , Algorithm 1 evaluates J ˉ x ( Π C u H ( U i , j ) ) \bar{J}_x(\Pi_{C_u^H}(U_{i,j})) J ˉ x ( Π C u H ( U i , j )) . Rollouts that still violate the state constraints, including obstacles, receive cost + ∞ +\infty + ∞ and weight zero. The implemented objective is therefore F x I I ( M ) : = − τ log E [ e − J ˉ x ( Π C u H ( M + ϵ ) ) / τ ] F_x^{\mathrm{II}}(M):=-\tau\log\mathbb{E}\left[e^{-\bar{J}_x(\Pi_{C_u^H}(M+\epsilon))/\tau}\right] F x II ( M ) := − τ log E [ e − J ˉ x ( Π C u H ( M + ϵ )) / τ ] , which (7) estimates at each center. All weights use the projected rollout costs, while weighted means and consensus points are formed from the unprojected samples and centers. The applied first input is projected onto C u C_u C u before being applied to the plant. For zero-weight cases, we use two fallback rules. If every rollout in a local batch is infeasible, we set its free-energy estimate to + ∞ +\infty + ∞ and its local output to the projected center. If all center weights vanish, we use the mean of the projected centers as the consensus. Appendix C.§ 1 gives the common numerical protocol.
Theoretical Analysis
We analyze the mean-field CBO-MPPI dynamics for a fixed open-loop problem. We establish well-posedness using the regularity of the free energy induced by Gaussian smoothing (Fazlyab et al., [6 ] ). The regularity and growth bounds verified here provide the basis for applying existing CBO analyses. In particular, Appendix § 7 .§ 2 develops a finite-accuracy global-convergence result under additional minimizer, initialization, and drift–diffusion assumptions.
We define the mean-field CBO-MPPI model for a fixed current state x ∈ R n x \in\mathbb{R}^{n} x ∈ R n and H , τ > 0 H, \tau> 0 H , τ > 0 , Σ ≻ 0 \Sigma\succ0 Σ ≻ 0 , with the associated admissible set C \mathcal{C} C . The model uses the exact, unprojected free energy F x F_x F x in (3) . Let P ( R D ) \mathcal{P}(\mathbb{R}^{D}) P ( R D ) denote the set of Borel probability measures on R D \mathbb{R}^{D} R D ; for 1 ≤ p < ∞ 1 \le p < \infty 1 ≤ p < ∞ , let P p ( R D ) \mathcal{P}_p(\mathbb{R}^{D}) P p ( R D ) be its finite-p p p th-moment subset, endowed with the Wasserstein-p p p distance W p W_p W p . The driving process B = ( B s ) s ≥ 0 B = (B_s)_{s \ge0} B = ( B s ) s ≥ 0 is a standard D D D -dimensional Brownian motion. The mean-field model is the McKean–Vlasov SDE
d M s = − λ ( M s − m α [ ρ s ] ) d s + σ c b o ∥ M s − m α [ ρ s ] ∥ d B s , ρ s = Law ( M s ) , (11) dM_s = -\lambda(M_s - m_\alpha[\rho_s])\,ds + \sigma_{\mathrm{cbo}}\lVert M_s - m_\alpha[\rho_s]\rVert\,dB_s,\qquad\rho_s = \operatorname{Law}(M_s), \tag*{(11)} d M s = − λ ( M s − m α [ ρ s ]) d s + σ cbo ∥ M s − m α [ ρ s ]∥ d B s , ρ s = Law ( M s ) , (11)
The law ρ s \rho_s ρ s represents the distribution of nominal centers at optimization time s s s . Here m α [ ρ ] m_\alpha[\rho] m α [ ρ ] is the free-energy-weighted consensus point for a particle distribution ρ ∈ P ( R D ) \rho\in\mathcal{P}(\mathbb{R}^{D}) ρ ∈ P ( R D ) , defined by
m α [ ρ ] : = ∫ R D M e − α F x ( M ) ρ ( d M ) ∫ R D e − α F x ( M ) ρ ( d M ) . (12) m_\alpha[\rho] := \frac{\int_{\mathbb{R}^{D}} M e^{-\alpha F_x(M)}\rho(dM)}{\int_{\mathbb{R}^{D}} e^{-\alpha F_x(M)}\rho(dM)}. \tag*{(12)} m α [ ρ ] := ∫ R D e − α F x ( M ) ρ ( d M ) ∫ R D M e − α F x ( M ) ρ ( d M ) . (12)
Under Assumptions 1 and 2, F x F_x F x is continuous and bounded below, so m α [ ρ ] ∈ R D m_\alpha[\rho] \in\mathbb{R}^{D} m α [ ρ ] ∈ R D is well defined for every ρ ∈ P 1 ( R D ) \rho\in\mathcal{P}_1(\mathbb{R}^{D}) ρ ∈ P 1 ( R D ) . The following theorem establishes well-posedness, using regularity and growth bounds for F x F_x F x proved in Appendix § 7 .
Theorem 5 (Well-posedness) Suppose Assumptions 1 and 2 hold, let ρ 0 ∈ P 4 ( R D ) \rho_0 \in\mathcal{P}_4(\mathbb{R}^{D}) ρ 0 ∈ P 4 ( R D ) , and let λ , σ c b o , α > 0 \lambda, \sigma_{\mathrm{cbo}}, \alpha> 0 λ , σ cbo , α > 0 . For every S > 0 S > 0 S > 0 , there exists a unique nonlinear strong process M M M with continuous sample paths satisfying (11) and Law ( M 0 ) = ρ 0 \operatorname{Law}(M_0) = \rho_0 Law ( M 0 ) = ρ 0 . It satisfies sup 0 ≤ s ≤ S E ∥ M s ∥ 4 < ∞ \sup_{0 \le s \le S} \mathbb{E}\lVert M_s\rVert^4 < \infty sup 0 ≤ s ≤ S E ∥ M s ∥ 4 < ∞ , and ρ s : = Law ( M s ) \rho_s := \operatorname{Law}(M_s) ρ s := Law ( M s ) belongs to C ( [ 0 , S ] , P 4 ( R D ) ) C([0,S],\mathcal{P}_4(\mathbb{R}^{D})) C ([ 0 , S ] , P 4 ( R D )) .
For the proof, Gaussian-kernel differentiation yields derivatives of F x F_x F x without requiring J x J_x J x itself to be differentiable. Compactness of C \mathcal{C} C gives global regularity and weighted-Lipschitz bounds, while Gaussian tails give quadratic growth. After translating a global minimizer to the origin, these bounds allow us to apply (Carrillo et al., [4 ] , Theorem 3.2) for existence, uniqueness, and the fourth-moment bound. Jensen’s inequality and the Burkholder–Davis–Gundy inequality then establish continuity of the law in W 4 W_4 W 4 . The complete proof is in Appendix § 7 .§ 1 , where we also establish finite-accuracy global convergence (Theorem 16 ) under additional assumptions.
Numerical Examples
We study four closed-loop tasks: Navigation, Many Obstacles, Pendulum, and Planar Arm. Navigation is the main route-selection example; Many Obstacles uses the same point mass in a cluttered geometry. Pendulum examines swing-up under actuator disturbances, and Planar Arm considers redundant manipulation with hard link-obstacle constraints.
Experimental Setting and Metrics
We compare CBO-MPPI with three baselines. MPPI performs one weighted-mean update per control period. Multi-start MPPI (MS-MPPI) independently applies a local MPPI update to each center after each of the first K − 1 K-1 K − 1 evaluation rounds. As in CBO-MPPI, its final output uses the MPPI weighted mean at the center with the smallest estimated free energy, and the final evaluated centers are shifted and jittered for the next control period. CBO-MPC treats each candidate control sequence as a particle, evaluates it with one rollout, applies CBO directly to trajectory cost [3 , 16 ] , and uses the first input of the lowest-cost particle. Appendix B.§ 1 gives pseudocode for the three baseline controllers.
We set the controller parameters as follows. We compare the methods at a fixed per-step rollout budget L t o t L_{\mathrm{tot}} L tot . MPPI evaluates L L L rollouts in one update, so L t o t = L L_{\mathrm{tot}}=L L tot = L . CBO-MPC evaluates N N N particles in each of K K K rounds, with one rollout per particle, so L t o t = N K L_{\mathrm{tot}}=NK L tot = N K . MS-MPPI and CBO-MPPI evaluate L L L local rollouts around each of N N N centers in each of K K K rounds, so L t o t = K N L L_{\mathrm{tot}=KNL} L tot = K N L . Using preliminary timing tests, we selected budgets to fit each task’s control period: L t o t = 131,072 L_{\mathrm{tot}}=131{,}072 L tot = 131 , 072 for Navigation, Many Obstacles, and Planar Arm and L t o t = 16,384 L_{\mathrm{tot}}=16{,}384 L tot = 16 , 384 for Pendulum. MS-MPPI matches CBO-MPPI in ( N , L , K ) (N,L,K) ( N , L , K ) , initialization, local perturbations, and warm-start jitter. For each comparison, the MPPI-based controllers share the temperature τ \tau τ and sampling covariance Σ \Sigma Σ . Appendices C.§ 1 –C.§ 5 detail the controller parameter settings.
For closed-loop evaluation, we define the realized cost J C L J_{\mathrm{CL}} J CL and the free energy F C L F_{\mathrm{CL}} F CL of the realized input sequence. All methods use the same T T T -step evaluation window within each task. For realized states x 0 , … , x T x_0,\ldots,x_T x 0 , … , x T and applied inputs U C L : = ( u 0 , … , u T − 1 ) ∈ R m T U_{\mathrm{CL}}:=(u_0,\ldots,u_{T-1})\in\mathbb{R}^{mT} U CL := ( u 0 , … , u T − 1 ) ∈ R m T , the realized cost is J C L : = ∑ k = 0 T − 1 ℓ ( x k + 1 , u k ) + g ( x T ) J_{\mathrm{CL}}:=\sum_{k=0}^{T-1}\ell(x_{k+1},u_k)+g(x_T) J CL := ∑ k = 0 T − 1 ℓ ( x k + 1 , u k ) + g ( x T ) . For L e L_{\mathrm{e}} L e perturbed sequences U C L + ϵ j U_{\mathrm{CL}}+\epsilon_j U CL + ϵ j with zero-mean Gaussian ϵ j ∈ R m T \epsilon_j\in\mathbb{R}^{mT} ϵ j ∈ R m T , the corresponding free energy is
F C L : = − τ log [ 1 L e ∑ j = 1 L e exp ( − J ‾ x 0 ( Π τ C ( U C L + ϵ j ) ) τ ) ] . (13) F_{\mathrm{CL}}:=-\tau\log\left[\frac{1}{L_{\mathrm{e}}}\sum_{j=1}^{L_{\mathrm{e}}}\exp\left(-\frac{\overline{J}_{x_0}\left(\Pi_{\tau}^{C}(U_{\mathrm{CL}}+\epsilon_j)\right)}{\tau}\right)\right]. \tag*{(13)} F CL := − τ log [ L e 1 j = 1 ∑ L e exp ( − τ J x 0 ( Π τ C ( U CL + ϵ j ) ) ) ] . (13)
We evaluate F C L F_{\mathrm{CL}} F CL after the control episode, with J ‾ x 0 \overline{J}_{x_0} J x 0 defined over this window. Appendix C.§ 1 specifies the task-dependent perturbation settings and evaluation sample counts.
All controllers and evaluation routines are implemented in Python using JAX/JAXlib 0.8.3 and NumPy 2.4.6. The numerical experiments used Apple M4 Max and NVIDIA RTX 6000 Ada hardware; implementation timings were measured on the former, a MacBook Pro with 128 GB unified memory. Each t c t r l t_{\mathrm{ctrl}} t ctrl is the median of 15 run-level control-cycle 95th percentiles, with the initial JIT compilation excluded. Code and reproducibility materials are available at https://github.com/inody/cbo_mppi .
Navigation: Objective Geometry and Closed-Loop Search
This task steers a planar point mass with position p k ∈ R 2 p_k\in\mathbb{R}^2 p k ∈ R 2 , velocity v k ∈ R 2 v_k\in\mathbb{R}^2 v k ∈ R 2 , and acceleration input u k ∈ R 2 u_k\in\mathbb{R}^2 u k ∈ R 2 through a narrow passage between two obstacles to a target. The dynamics are p k + 1 = p k + Δ t v k p_{k+1}=p_k+\Delta t\,v_k p k + 1 = p k + Δ t v k and v k + 1 = v k + Δ t u k v_{k+1}=v_k+\Delta t\,u_k v k + 1 = v k + Δ t u k , with time step Δ t = 0.1 \Delta t=0.1 Δ t = 0.1 . The initial state is ( p 0 , v 0 ) = ( − 4 , 0 , 0 , 0 ) (p_0,v_0)=(-4,0,0,0) ( p 0 , v 0 ) = ( − 4 , 0 , 0 , 0 ) , and the target position is ( 4 , 0 ) (4,0) ( 4 , 0 ) . Two obstacles of radius 0.5 are centered at ( 0 , ± 0.85 ) (0,\pm0.85) ( 0 , ± 0.85 ) . We encode obstacle avoidance in C x C_x C x by excluding states whose positions lie inside either obstacle. The cost penalizes goal-position error, velocity, control effort, and input increments, with terminal penalties on position error and velocity. The narrow central passage and the longer detours are competing routes.
Route family and objective preference. We first compare objectives on artificially constructed open-loop routes before examining trajectories produced by feedback control. The routes are parameterized by signed crossing height h h h : h = 0 h = 0 h = 0 passes through the central gap; positive and negative h h h bend the routes upward and downward. Figure 1 (left) shows representative routes. For each route’s input sequence, we evaluate the nominal trajectory cost and the projected free energy at τ = 15 \tau= 15 τ = 15 and 300. Figure 1 (right) plots these values against h h h . Perturbations around the narrow center route are more likely to encounter obstacles than those around wide detours, increasing its free energy. The nominal cost and low-temperature free energy favor the center route. The high-temperature free-energy minimum shifts to a detour. Appendix C.§ 2 gives the task specifications, route parameterization, and unnormalized evaluation.
Figure 1. Navigation objective geometry. Left: prescribed open-loop routes colored by signed route height h h h . Right: nominal cost and estimated projected free energy along the same family, with each curve mapped to [ 0 , 1 ] [0,1] [ 0 , 1 ] using its own minimum and maximum. In the legend, F ^ 15 Π \widehat{F}^{\Pi}_{15} F 15 Π and F ^ 300 Π \widehat{F}^{\Pi}_{300} F 300 Π denote the free energy at τ = 15 \tau= 15 τ = 15 and τ = 300 \tau= 300 τ = 300 .
Closed-loop search. Table 1 compares median J C L J_{\mathrm{CL}} J CL and F C L F_{\mathrm{CL}} F CL for all four methods at both temperatures. Figure 2 shows trajectories for MPPI, CBO-MPC, and CBO-MPPI. At τ = 15 \tau= 15 τ = 15 , the three plotted methods primarily use the center passage. At τ = 300 \tau= 300 τ = 300 , MPPI uses that passage in all 100 trials, whereas CBO-MPPI detours in 94/100 trials and has greater median minimum obstacle clearance (0.717 versus 0.353). CBO-MPPI’s lower F C L F_{\mathrm{CL}} F CL at this temperature accompanies realized cost close to MPPI’s. MS-MPPI also lowers F C L F_{\mathrm{CL}} F CL relative to MPPI at high temperature and incurs a larger J C L J_{\mathrm{CL}} J CL ; at low temperature, it improves neither cost. CBO-MPPI has lower F C L F_{\mathrm{CL}} F CL than MS-MPPI at both temperatures. CBO-MPC has the lowest J C L J_{\mathrm{CL}} J CL at both temperatures and the lowest F C L F_{\mathrm{CL}} F CL at τ = 15 \tau= 15 τ = 15 ; CBO-MPPI has the lowest F C L F_{\mathrm{CL}} F CL at τ = 300 \tau= 300 τ = 300 .
Figure 2. Navigation trajectories for MPPI, CBO-MPC, and CBO-MPPI at equal rollout budget, with all 100 trials per condition.
Table 1. Median J C L J_{\mathrm{CL}} J CL and F C L F_{\mathrm{CL}} F CL over 100 trials per task at equal rollout budgets. MS-MPPI uses persistent independent centers; CBO-MPC is fixed across temperatures, with only F C L F_{\mathrm{CL}} F CL reevaluated at each τ \tau τ .
CBO-MPPI’s route selection follows the temperature-dependent preference of the open-loop landscape; MPPI remains in the center passage. CBO-MPC also obtains smaller F C L F_{\mathrm{CL}} F CL with center trajectories by accumulating less position-error cost and achieving higher perturbation feasibility than MPPI. The open-loop landscape therefore helps explain CBO-MPPI’s route selection, and F C L F_{\mathrm{CL}} F CL additionally reflects closed-loop trajectory cost and perturbation feasibility.
Complementary Tasks: Many Obstacles, Pendulum, and Planar Arm
We conduct experiments on three additional tasks and summarize the four-task comparison in Table 1 . At the selected budgets, CBO-MPPI’s t c t r l t_{\mathrm{ctrl}} t ctrl is below the control period on all four tasks and is comparable to baseline values. Across the obstacle-avoidance tasks, the high-temperature results show larger median minimum clearance for CBO-MPPI than for CBO-MPC: 0.717 versus 0.319 on Navigation, 0.458 versus 0.080 on Many Obstacles, and 0.132 versus 0.003 on Planar Arm. Appendices C.§ 2 –C.§ 5 give the detailed setups and complete results for all four tasks.
Many Obstacles. This task steers the same point mass through a cluttered obstacle field. At τ = 15 \tau= 15 τ = 15 , CBO-MPC has the lowest median F C L F_{\mathrm{CL}} F CL , followed by MPPI, CBO-MPPI, and MS-MPPI. At τ = 3000 \tau= 3000 τ = 3000 , CBO-MPPI has the lowest F C L F_{\mathrm{CL}} F CL and MS-MPPI is second; both remain below MPPI and CBO-MPC. CBO-MPC and MPPI attain smaller J C L J_{\mathrm{CL}} J CL than CBO-MPPI at high temperature, while MS-MPPI has the largest realized cost. Thus CBO-MPPI’s high-temperature free-energy advantage persists in the cluttered setting, with higher realized cost than MPPI and CBO-MPC.
Pendulum. This task swings a pendulum from downward to upright under bounded inputs and shared actuator disturbances. CBO-MPPI has the lowest median F C L F_{\mathrm{CL}} F CL at both τ = 1 \tau= 1 τ = 1 and 10. MS-MPPI has the highest F C L F_{\mathrm{CL}} F CL at low temperature. At high temperature, it improves on MPPI and remains above CBO-MPC and CBO-MPPI. At τ = 1 \tau= 1 τ = 1 , CBO-MPPI’s smaller free energy comes with larger J C L J_{\mathrm{CL}} J CL than MPPI and CBO-MPC. At τ = 10 \tau= 10 τ = 10 , CBO-MPPI lowers both costs relative to MPPI and MS-MPPI; CBO-MPC still has smaller J C L J_{\mathrm{CL}} J CL . These Pendulum trials extend the observed free-energy improvement beyond obstacle-avoidance tasks.
Planar Arm. This task moves a three-link arm around obstacles to a target end-effector position. At τ = 3 \tau= 3 τ = 3 , MS-MPPI has the lowest median F C L F_{\mathrm{CL}} F CL and J C L J_{\mathrm{CL}} J CL ; CBO-MPC is second, followed by CBO-MPPI and MPPI. At τ = 300 \tau= 300 τ = 300 , CBO-MPPI attains smaller F C L F_{\mathrm{CL}} F CL than MS-MPPI, and both remain below CBO-MPC and MPPI. CBO-MPC has the smallest J C L J_{\mathrm{CL}} J CL at this temperature.
Parameter and Horizon Sensitivity
We assess whether CBO-MPPI’s free-energy advantage over MPPI persists beyond the reference settings, varying first the sampling parameters and then the prediction horizon. We vary the inverse temperature α \alpha α and the ratio κ = D c c b o 2 / ( 2 λ ) \kappa= Dc_{\mathrm{cbo}}^2/(2\lambda) κ = D c cbo 2 / ( 2 λ ) one at a time, with 20 paired trials per setting. Across
80 task–temperature–parameter settings, the median paired F C L F_{\mathrm{CL}} F CL difference (CBO-MPPI minus MPPI) is negative at 62, including all 40 high-temperature settings. Thus the high-temperature advantage is not tied to the reference parameter choice. At low temperature, several settings reverse the comparison, and Pendulum at τ = 1 \tau= 1 τ = 1 favors MPPI throughout.
The horizon sweep shows where consensus helps. At short horizons, MS-MPPI can match or outperform CBO-MPPI. At the longest tested horizons, however, CBO-MPPI succeeds in 17/20 trials versus MS-MPPI’s 11/20 on Navigation and 16/20 versus MS-MPPI’s 1/20 on Pendulum. At the reference horizons, CBO-MPPI has lower realized cost and higher success rates than MS-MPPI on all three tasks; its free energy is lower on Navigation and Planar Arm and nearly equal on Pendulum. Appendices C.6 and C.7 give the grids and complete results.
Conclusion
We proposed CBO-MPPI for predictive-control problems in which hard constraints can make the trajectory cost discontinuous and create multiple search basins. Gaussian perturbations transform this cost into an MPPI free energy that rewards sampling neighborhoods containing feasible, low-cost trajectories. CBO-MPPI searches this smoothed objective with a population of coupled centers and obtains the control from the selected center’s MPPI weighted mean. For the mean-field model of a fixed open-loop problem with exact, unprojected free energy, we established well-posedness and, under additional assumptions, convergence toward a free-energy minimizer. The simulations support this design: when the temperature is high enough for perturbation feasibility to matter, the consensus search reaches lower free energy than MPPI and MS-MPPI and keeps trajectories farther from obstacles than CBO applied to the nominal cost, and its advantage over MPPI persists across the tested parameter ranges. Extending the theory to finite-particle and finite-rollout implementations, input projection, and closed-loop stability remains future work.
Generative AI Use
Generative AI tools were used to assist with numerical computations and writing. The authors reviewed and verified all AI-assisted content and take responsibility for the final content of the paper.
Acknowledgments
D. Kalise is supported by the Air Force Office of Scientific Research under award number FA8655-26-1-B011. Y. Huang is supported by a Research Fellowship from the Royal Commission for the Exhibition of 1851.
Mean-Field Convergence and Proofs
This appendix proceeds in two steps. Subsection A.1 establishes the free-energy properties needed for well-posedness and proves Theorem 5 . Subsection A.2 introduces the additional assumptions and objective conditions under which Theorem 16 establishes finite-accuracy global convergence.
Free-Energy Properties and Well-Posedness
We first verify the regularity and growth properties of F x F_x F x needed for well-posedness and then prove Theorem 5 . Additional conditions for finite-accuracy global convergence are introduced in Subsection A.2. We begin by introducing the partition function
Z ( M ) : = ∫ C φ Σ ( U − M ) exp ( − J x ( U ) τ ) d U . Z(M) := \int_{\mathcal{C}} \varphi_{\Sigma}(U-M)\exp\left(-\frac{J_x(U)}{\tau}\right)\,dU. Z ( M ) := ∫ C φ Σ ( U − M ) exp ( − τ J x ( U ) ) d U .
so that F x ( M ) = − τ log Z ( M ) F_x(M)=-\tau\log Z(M) F x ( M ) = − τ log Z ( M ) .
Lemma 6 (Positivity and finiteness of the partition function) Under Assumptions 1 and 2, for every M ∈ R D M\in\mathbb{R}^D M ∈ R D ,
0 < Z ( M ) < ∞ . 0<Z(M)<\infty. 0 < Z ( M ) < ∞. Consequently, F x ( M ) F_x(M) F x ( M ) is well-defined for all M ∈ R D M\in\mathbb{R}^D M ∈ R D .
Proof By Assumption 2,
e − J max / τ ≤ exp ( − J x ( U ) τ ) ≤ e − J min / τ , U ∈ C . e^{-J_{\max}/\tau}\leq\exp\left(-\frac{J_x(U)}{\tau}\right)\leq e^{-J_{\min}/\tau},\qquad U\in\mathcal{C}. e − J m a x / τ ≤ exp ( − τ J x ( U ) ) ≤ e − J m i n / τ , U ∈ C .
Since the Gaussian density φ Σ \varphi_{\Sigma} φ Σ is everywhere positive and C \mathcal{C} C has positive Lebesgue measure,
Z ( M ) ≥ e − J max / τ ∫ C φ Σ ( U − M ) d U > 0. Z(M)\geq e^{-J_{\max}/\tau}\int_{\mathcal{C}}\varphi_{\Sigma}(U-M)\,dU>0. Z ( M ) ≥ e − J m a x / τ ∫ C φ Σ ( U − M ) d U > 0.
On the other hand,
Z ( M ) ≤ e − J min / τ ∫ C φ Σ ( U − M ) d U ≤ e − J min / τ < ∞ , Z(M)\leq e^{-J_{\min}/\tau}\int_{\mathcal{C}}\varphi_{\Sigma}(U-M)\,dU\leq e^{-J_{\min}/\tau}<\infty, Z ( M ) ≤ e − J m i n / τ ∫ C φ Σ ( U − M ) d U ≤ e − J m i n / τ < ∞ ,
which completes the proof. ▪
We next show that Gaussian convolution makes F x F_x F x smooth, even when J x J_x J x is discontinuous.
Lemma 7 (Smoothness of the MPPI free energy) Under Assumptions 1 and 2,
Z ∈ C ∞ ( R D ) , F x ∈ C ∞ ( R D ) . Z\in C^\infty(\mathbb{R}^D),\qquad F_x\in C^\infty(\mathbb{R}^D). Z ∈ C ∞ ( R D ) , F x ∈ C ∞ ( R D ) . Proof Set
h ( U ) : = 1 C ( U ) exp ( − J x ( U ) τ ) . h(U):=\mathbf{1}_{\mathcal{C}}(U)\exp\left(-\frac{J_x(U)}{\tau}\right). h ( U ) := 1 C ( U ) exp ( − τ J x ( U ) ) . By Assumption 2, h h h is a bounded measurable function, and by Assumption 1 it has compact support. Then
Z ( M ) = ∫ R D φ Σ ( U − M ) h ( U ) d U . Z(M)=\int_{\mathbb{R}^D}\varphi_{\Sigma}(U-M)h(U)\,dU. Z ( M ) = ∫ R D φ Σ ( U − M ) h ( U ) d U . The Gaussian density φ Σ \varphi_{\Sigma} φ Σ belongs to C ∞ C^\infty C ∞ , and each of its derivatives is a product of a polynomial and a Gaussian function. Since h h h is bounded with compact support, differentiation and integration may be interchanged for any multi-index β \beta β :
∂ M β Z ( M ) = ∫ R D ∂ M β φ Σ ( U − M ) h ( U ) d U . \partial_M^\beta Z(M)=\int_{\mathbb{R}^D}\partial_M^\beta\varphi_{\Sigma}(U-M)h(U)\,dU. ∂ M β Z ( M ) = ∫ R D ∂ M β φ Σ ( U − M ) h ( U ) d U .
Hence Z ∈ C ∞ ( R D ) Z \in C^{\infty}(\mathbb{R}^{D}) Z ∈ C ∞ ( R D ) . Since Z ( M ) > 0 Z(M)>0 Z ( M ) > 0 by Lemma 6 , the function log Z ( M ) \log Z(M) log Z ( M ) is also C ∞ C^{\infty} C ∞ , and therefore
F x ( M ) = − τ log Z ( M ) ∈ C ∞ ( R D ) . F_x(M)=-\tau\log Z(M)\in C^{\infty}(\mathbb{R}^{D}). F x ( M ) = − τ log Z ( M ) ∈ C ∞ ( R D ) .
For M ∈ R D M\in\mathbb{R}^{D} M ∈ R D , define the tilted distribution
ρ M ( d U ) : = φ Σ ( U − M ) exp ( − J x ( U ) / τ ) 1 C ( U ) d U Z ( M ) . \rho_M(dU):=\frac{\varphi_{\Sigma}(U-M)\exp\left(-J_x(U)/\tau\right)\mathbf{1}_{\mathcal{C}}(U)\,dU}{Z(M)}. ρ M ( d U ) := Z ( M ) φ Σ ( U − M ) exp ( − J x ( U ) / τ ) 1 C ( U ) d U .
Then E ρ M [ U ] = Φ ( M ) \mathbb{E}_{\rho_M}[U]=\Phi(M) E ρ M [ U ] = Φ ( M ) , where Φ \Phi Φ is the exact MPPI update already defined in (6) . We recall the Gaussian gradient and Hessian identities of Fazlyab et al., [6 ] , whose θ = μ \theta=\mu θ = μ , u u u , f 0 f_0 f 0 , C \mathcal{C} C , and π θ ( u ) \pi_\theta(u) π θ ( u ) correspond to our M M M , U U U , J x J_x J x , C \mathcal{C} C , and φ Σ ( U − M ) \varphi_{\Sigma}(U-M) φ Σ ( U − M ) , respectively. That reference assumes a continuous cost, but its derivation uses only positivity of Z Z Z and differentiation under the integral, which Lemmas 6 and 7 provide for bounded Borel J x J_x J x .
Proposition 8 (Gaussian free-energy identities (6) ). Under Assumptions 1 and 2,
∇ F x ( M ) = τ Σ − 1 ( M − E ρ M [ U ] ) , (14) \nabla F_x(M)=\tau\Sigma^{-1}\left(M-\mathbb{E}_{\rho_M}[U]\right), \tag*{(14)} ∇ F x ( M ) = τ Σ − 1 ( M − E ρ M [ U ] ) , (14) and
∇ 2 F x ( M ) = τ Σ − 1 − τ Σ − 1 Cov ρ M ( U ) Σ − 1 . (15) \nabla^2 F_x(M)=\tau\Sigma^{-1}-\tau\Sigma^{-1}\operatorname{Cov}_{\rho_M}(U)\Sigma^{-1}. \tag*{(15)} ∇ 2 F x ( M ) = τ Σ − 1 − τ Σ − 1 Cov ρ M ( U ) Σ − 1 . (15)
Proof. See Fazlyab et al., [6 ] .
We next show that F x F_x F x is coercive, i.e., F x ( M ) → + ∞ F_x(M)\to+\infty F x ( M ) → + ∞ as ∥ M ∥ → ∞ \lVert M\rVert\to\infty ∥ M ∥ → ∞ . This ensures that the objective does not attain values near its global minimum far from the minimizer and implies the existence of a global minimizer. We use the Σ − 1 \Sigma^{-1} Σ − 1 -weighted distance notation
∥ z ∥ Σ − 1 2 : = z T Σ − 1 z , dist Σ − 1 ( M , C ) : = inf U ∈ C ∥ M − U ∥ Σ − 1 . \lVert z\rVert_{\Sigma^{-1}}^2:=z^{\mathsf{T}}\Sigma^{-1}z,\qquad\operatorname{dist}_{\Sigma^{-1}}(M,\mathcal{C}):=\inf_{U\in\mathcal{C}}\lVert M-U\rVert_{\Sigma^{-1}}. ∥ z ∥ Σ − 1 2 := z T Σ − 1 z , dist Σ − 1 ( M , C ) := U ∈ C inf ∥ M − U ∥ Σ − 1 .
Lemma 9 (Coercivity of the MPPI free energy). Under Assumptions 1 and 2, there exists a constant c 0 ∈ R c_0\in\mathbb{R} c 0 ∈ R such that for all M ∈ R D M\in\mathbb{R}^{D} M ∈ R D ,
F x ( M ) ≥ τ 2 dist Σ − 1 ( M , C ) 2 − c 0 . (16) F_x(M)\ge\frac{\tau}{2}\operatorname{dist}_{\Sigma^{-1}}(M,\mathcal{C})^2-c_0. \tag*{(16)} F x ( M ) ≥ 2 τ dist Σ − 1 ( M , C ) 2 − c 0 . (16) In particular,
F x ( M ) → + ∞ as ∥ M ∥ → ∞ . F_x(M)\to+\infty\quad\text{as }\lVert M\rVert\to\infty. F x ( M ) → + ∞ as ∥ M ∥ → ∞.
Proof. The Gaussian density satisfies
φ Σ ( U − M ) = 1 ( 2 π ) D / 2 det ( Σ ) 1 / 2 exp ( − 1 2 ∥ U − M ∥ Σ − 1 2 ) . \varphi_{\Sigma}(U-M)=\frac{1}{(2\pi)^{D/2}\det(\Sigma)^{1/2}}\exp\left(-\frac{1}{2}\lVert U-M\rVert_{\Sigma^{-1}}^2\right). φ Σ ( U − M ) = ( 2 π ) D /2 det ( Σ ) 1/2 1 exp ( − 2 1 ∥ U − M ∥ Σ − 1 2 ) .
For any U ∈ C U\in\mathcal{C} U ∈ C ,
∥ U − M ∥ Σ − 1 2 ≥ dist Σ − 1 ( M , C ) 2 . \lVert U-M\rVert_{\Sigma^{-1}}^2\ge\operatorname{dist}_{\Sigma^{-1}}(M,\mathcal{C})^2. ∥ U − M ∥ Σ − 1 2 ≥ dist Σ − 1 ( M , C ) 2 .
so
φ Σ ( U − M ) ≤ 1 ( 2 π ) D / 2 det ( Σ ) 1 / 2 exp ( − 1 2 dist Σ − 1 ( M , C ) 2 ) . \varphi_{\Sigma}(U-M)\le\frac{1}{(2\pi)^{D/2}\det(\Sigma)^{1/2}}\exp\left(-\frac{1}{2}\operatorname{dist}_{\Sigma^{-1}}(M,\mathcal{C})^2\right). φ Σ ( U − M ) ≤ ( 2 π ) D /2 det ( Σ ) 1/2 1 exp ( − 2 1 dist Σ − 1 ( M , C ) 2 ) .
Combined with the bound exp ( − J x ( U ) / τ ) ≤ exp ( − J min / τ ) \exp(-J_x(U)/\tau)\le\exp(-J_{\min}/\tau) exp ( − J x ( U ) / τ ) ≤ exp ( − J m i n / τ ) from Assumption 2,
Z ( M ) = ∫ C φ Σ ( U − M ) exp ( − J x ( U ) τ ) d U ≤ ∣ C ∣ ( 2 π ) D / 2 det ( Σ ) 1 / 2 exp ( − J min τ ) exp ( − 1 2 dist Σ − 1 ( M , C ) 2 ) , \begin{aligned}
Z(M)=\int_{\mathcal{C}}\varphi_{\Sigma}(U-M)\exp\left(-\frac{J_x(U)}{\tau}\right)\,dU \\
&\le\frac{|\mathcal{C}|}{(2\pi)^{D/2}\det(\Sigma)^{1/2}}\exp\left(-\frac{J_{\min}}{\tau}\right)\exp\left(-\frac{1}{2}\operatorname{dist}_{\Sigma^{-1}}(M,\mathcal{C})^2\right),
\end{aligned} Z ( M ) = ∫ C φ Σ ( U − M ) exp ( − τ J x ( U ) ) d U ≤ ( 2 π ) D /2 det ( Σ ) 1/2 ∣ C ∣ exp ( − τ J m i n ) exp ( − 2 1 dist Σ − 1 ( M , C ) 2 ) ,
where ∣ C ∣ |\mathcal{C}| ∣ C ∣ denotes the Lebesgue measure of C \mathcal{C} C . Applying − τ log -\tau\log − τ log to both sides,
F x ( M ) = − τ log Z ( M ) ≥ τ 2 dist Σ − 1 ( M , C ) 2 − c 0 , F_x(M)=-\tau\log Z(M)\ge\frac{\tau}{2}\operatorname{dist}_{\Sigma^{-1}}(M,\mathcal{C})^2-c_0, F x ( M ) = − τ log Z ( M ) ≥ 2 τ dist Σ − 1 ( M , C ) 2 − c 0 ,
where c 0 c_0 c 0 depends on C \mathcal{C} C , Σ \Sigma Σ , τ \tau τ , and J min J_{\min} J m i n . Since C \mathcal{C} C is compact,
∥ M ∥ → ∞ ⟹ dist Σ − 1 ( M , C ) → ∞ , \lVert M\rVert\to\infty\quad\Longrightarrow\quad\operatorname{dist}_{\Sigma^{-1}}(M,\mathcal{C})\to\infty, ∥ M ∥ → ∞ ⟹ dist Σ − 1 ( M , C ) → ∞ ,
and therefore F x ( M ) → + ∞ F_x(M)\to+\infty F x ( M ) → + ∞ .
Corollary 10 (Existence of a global minimizer) Under Assumptions 1 and 2 , F x F_x F x has at least one global minimizer; that is,
argmin M ∈ R D F x ( M ) ≠ ∅ . \underset{M\in\mathbb{R}^{D}}{\operatorname{argmin}} F_x(M)\ne\varnothing. M ∈ R D argmin F x ( M ) = ∅ .
Proof. F x F_x F x is continuous by Lemma 7 and coercive by Lemma 9 . Hence there exists R > 0 R>0 R > 0 large enough that
inf M ∈ R D F x ( M ) = inf ∥ M ∥ ≤ R F x ( M ) . \inf_{M\in\mathbb{R}^{D}}F_x(M)=\inf_{\lVert M\rVert\le R}F_x(M). M ∈ R D inf F x ( M ) = ∥ M ∥ ≤ R inf F x ( M ) .
Since the closed ball { M ∈ R D : ∥ M ∥ ≤ R } \{M\in\mathbb{R}^{D}:\lVert M\rVert\le R\} { M ∈ R D : ∥ M ∥ ≤ R } is compact, the Weierstrass theorem guarantees that the continuous function F x F_x F x attains its minimum on this ball.
Fix a minimizer
M ⋆ ∈ argmin M ∈ R D F x ( M ) . M^\star\in\underset{M\in\mathbb{R}^{D}}{\operatorname{argmin}}F_x(M). M ⋆ ∈ M ∈ R D argmin F x ( M ) .
The Gaussian free-energy structure also provides the global regularity and growth estimates required by the standard CBO well-posedness theory.
Lemma 11 (Global regularity and quadratic growth of the MPPI free energy) Under Assumptions 1 and 2 , there exists L F < ∞ L_F<\infty L F < ∞ such that
sup M ∈ R D ∥ ∇ 2 F x ( M ) ∥ ≤ L F . \sup_{M\in\mathbb{R}^{D}}\lVert\nabla^2F_x(M)\rVert\le L_F. M ∈ R D sup ∥ ∇ 2 F x ( M )∥ ≤ L F . For this fixed global minimizer M ⋆ M^\star M ⋆ , define
F ~ x ( z ) : = F x ( M ⋆ + z ) − F x ( M ⋆ ) . \widetilde{F}_x(z):=F_x(M^\star+z)-F_x(M^\star). F x ( z ) := F x ( M ⋆ + z ) − F x ( M ⋆ ) .
Then, for all z , y ∈ R D z,y \in\mathbb{R}^{D} z , y ∈ R D ,
0 ≤ F ~ x ( z ) ≤ L F 2 ∥ z ∥ 2 , ∣ F ~ x ( z ) − F ~ x ( y ) ∣ ≤ L F ( ∥ z ∥ + ∥ y ∥ ) ∥ z − y ∥ . (17) 0 \le\widetilde{F}_{x}(z) \le\frac{L_{F}}{2}\lVert z\rVert^{2}, \qquad\lvert\widetilde{F}_{x}(z)-\widetilde{F}_{x}(y)\rvert\le L_{F}(\lVert z\rVert+\lVert y\rVert)\lVert z-y\rVert. \tag*{(17)} 0 ≤ F x ( z ) ≤ 2 L F ∥ z ∥ 2 , ∣ F x ( z ) − F x ( y )∣ ≤ L F (∥ z ∥ + ∥ y ∥) ∥ z − y ∥ . (17)
Moreover, there exist R > 0 R>0 R > 0 and c l > 0 c_l>0 c l > 0 such that
F ~ x ( z ) ≥ c l ∥ z ∥ 2 , ∥ z ∥ ≥ R . (18) \widetilde{F}_{x}(z) \ge c_l\lVert z\rVert^{2}, \qquad\lVert z\rVert\ge R. \tag*{(18)} F x ( z ) ≥ c l ∥ z ∥ 2 , ∥ z ∥ ≥ R . (18)
Proof. We use the Hessian identity recalled in Proposition 8 . Since ρ M \rho_M ρ M is supported on the compact set C \mathcal{C} C , choose R C , 0 < ∞ R_{\mathcal{C},0}<\infty R C , 0 < ∞ such that ∥ U ∥ ≤ R C , 0 \lVert U\rVert\le R_{\mathcal{C},0} ∥ U ∥ ≤ R C , 0 on C \mathcal{C} C . For every unit vector v v v ,
v T Cov ρ M ( U ) v = Var ρ M ( v T U ) ≤ E ρ M ∣ v T U ∣ 2 ≤ R C , 0 2 . v^{\mathsf{T}}\operatorname{Cov}_{\rho_M}(U)v=\operatorname{Var}_{\rho_M}(v^{\mathsf{T}}U)\le\mathbb{E}_{\rho_M}\lvert v^{\mathsf{T}}U\rvert^{2}\le R_{\mathcal{C},0}^{2}. v T Cov ρ M ( U ) v = Var ρ M ( v T U ) ≤ E ρ M ∣ v T U ∣ 2 ≤ R C , 0 2 .
Thus ∥ Cov ρ M ( U ) ∥ ≤ R C , 0 2 \lVert\operatorname{Cov}_{\rho_M}(U)\rVert\le R_{\mathcal{C},0}^{2} ∥ Cov ρ M ( U )∥ ≤ R C , 0 2 uniformly in M M M , and (15) gives, for example,
L F : = τ ∥ Σ − 1 ∥ + τ R C , 0 2 ∥ Σ − 1 ∥ 2 . L_F := \tau\lVert\Sigma^{-1}\rVert+\tau R_{\mathcal{C},0}^{2}\lVert\Sigma^{-1}\rVert^{2}. L F := τ ∥ Σ − 1 ∥ + τ R C , 0 2 ∥ Σ − 1 ∥ 2 .
Since F x F_x F x is smooth and M ⋆ M^{\star} M ⋆ is a minimizer, F ~ x ( 0 ) = 0 \widetilde{F}_{x}(0)=0 F x ( 0 ) = 0 and ∇ F ~ x ( 0 ) = 0 \nabla\widetilde{F}_{x}(0)=0 ∇ F x ( 0 ) = 0 . The global Hessian bound therefore yields
∥ ∇ F ~ x ( z ) ∥ ≤ L F ∥ z ∥ . \lVert\nabla\widetilde{F}_{x}(z)\rVert\le L_F\lVert z\rVert. ∥ ∇ F x ( z )∥ ≤ L F ∥ z ∥ .
Taylor’s formula at the origin gives the quadratic upper bound in (17) . Integrating the gradient along the segment from y y y to z z z gives
∣ F ~ x ( z ) − F ~ x ( y ) ∣ ≤ L F ∫ 0 1 ∥ y + t ( z − y ) ∥ d t ∥ z − y ∥ ≤ L F ( ∥ z ∥ + ∥ y ∥ ) ∥ z − y ∥ , \lvert\widetilde{F}_{x}(z)-\widetilde{F}_{x}(y)\rvert\le L_F\int_{0}^{1}\lVert y+t(z-y)\rVert\,\mathrm{d}t\,\lVert z-y\rVert\le L_F(\lVert z\rVert+\lVert y\rVert)\lVert z-y\rVert, ∣ F x ( z ) − F x ( y )∣ ≤ L F ∫ 0 1 ∥ y + t ( z − y )∥ d t ∥ z − y ∥ ≤ L F (∥ z ∥ + ∥ y ∥) ∥ z − y ∥ ,
which is the stated weighted-Lipschitz estimate.
Finally, let
R C : = sup U ∈ C ∥ U − M ⋆ ∥ < ∞ . R_{\mathcal{C}}:=\sup_{U\in\mathcal{C}}\lVert U-M^{\star}\rVert<\infty. R C := U ∈ C sup ∥ U − M ⋆ ∥ < ∞.
Whenever ∥ z ∥ ≥ R C \lVert z\rVert\ge R_{\mathcal{C}} ∥ z ∥ ≥ R C , the reverse triangle inequality gives, for every U ∈ C U\in\mathcal{C} U ∈ C ,
∥ M ⋆ + z − U ∥ Σ − 1 2 ≥ λ min ( Σ − 1 ) ( ∥ z ∥ − R C ) 2 . \lVert M^{\star}+z-U\rVert_{\Sigma^{-1}}^{2}\ge\lambda_{\min}(\Sigma^{-1})(\lVert z\rVert-R_{\mathcal{C}})^{2}. ∥ M ⋆ + z − U ∥ Σ − 1 2 ≥ λ m i n ( Σ − 1 ) (∥ z ∥ − R C ) 2 .
Combining this inequality with (16) gives
F ~ x ( z ) ≥ τ 2 λ min ( Σ − 1 ) ( ∥ z ∥ − R C ) 2 − c 0 − F x ( M ⋆ ) . \widetilde{F}_{x}(z)\ge\frac{\tau}{2}\lambda_{\min}(\Sigma^{-1})(\lVert z\rVert-R_{\mathcal{C}})^{2}-c_0-F_x(M^{\star}). F x ( z ) ≥ 2 τ λ m i n ( Σ − 1 ) (∥ z ∥ − R C ) 2 − c 0 − F x ( M ⋆ ) .
For sufficiently large ∥ z ∥ \lVert z\rVert ∥ z ∥ , the constants can be absorbed into the quadratic term, yielding (18) . ■
Proof of well-posedness. The law ρ s : = Law ( M s ) \rho_s := \operatorname{Law}(M_s) ρ s := Law ( M s ) of the process in Theorem 5 is a weak solution of the nonlinear Fokker–Planck equation
∂ s ρ s = λ ∇ M ⋅ [ ( M − m α [ ρ s ] ) ρ s ] + σ c b o 2 2 Δ M [ ∥ M − m α [ ρ s ] ∥ 2 ρ s ] . (19) \partial_s\rho_s = \lambda\nabla_M \cdot[(M-m_\alpha[\rho_s])\rho_s] + \frac{\sigma_{\mathrm{cbo}}^2}{2}\Delta_M [\lVert M-m_\alpha[\rho_s]\rVert^2\rho_s]. \tag*{(19)} ∂ s ρ s = λ ∇ M ⋅ [( M − m α [ ρ s ]) ρ s ] + 2 σ cbo 2 Δ M [∥ M − m α [ ρ s ] ∥ 2 ρ s ] . (19)
For completeness, the weak formulation of (19) is as follows: for every ϕ ∈ C c ∞ ( R D ) \phi\in C^\infty_{\mathrm{c}}(\mathbb{R}^D) ϕ ∈ C c ∞ ( R D ) and s ∈ [ 0 , S ] s \in[0,S] s ∈ [ 0 , S ] ,
∫ R D ϕ ( M ) ρ s ( d M ) − ∫ R D ϕ ( M ) ρ 0 ( d M ) = − λ ∫ 0 s ∫ R D ( M − m α [ ρ s ′ ] ) ⋅ ∇ ϕ ( M ) ρ s ′ ( d M ) d s ′ + σ c b o 2 2 ∫ 0 s ∫ R D ∥ M − m α [ ρ s ′ ] ∥ 2 Δ ϕ ( M ) ρ s ′ ( d M ) d s ′ . (20) \begin{aligned}
\int_{\mathbb{R}^D}\phi(M)\rho_s(dM)-\int_{\mathbb{R}^D}\phi(M)\rho_0(dM)
&=-\lambda\int_0^s\int_{\mathbb{R}^D}(M-m_\alpha[\rho_{s'}])\cdot\nabla\phi(M)\rho_{s'}(dM)\,ds'\\
&\quad+\frac{\sigma_{\mathrm{cbo}}^2}{2}\int_0^s\int_{\mathbb{R}^D}\lVert M-m_\alpha[\rho_{s'}]\rVert^2\Delta\phi(M)\rho_{s'}(dM)\,ds'. \tag*{(20)}
\end{aligned} ∫ R D ϕ ( M ) ρ s ( d M ) − ∫ R D ϕ ( M ) ρ 0 ( d M ) = − λ ∫ 0 s ∫ R D ( M − m α [ ρ s ′ ]) ⋅ ∇ ϕ ( M ) ρ s ′ ( d M ) d s ′ + 2 σ cbo 2 ∫ 0 s ∫ R D ∥ M − m α [ ρ s ′ ] ∥ 2 Δ ϕ ( M ) ρ s ′ ( d M ) d s ′ . (20)
Proof [Proof of Theorem 5 ] Use the global minimizer M ⋆ M^\star M ⋆ fixed after Corollary 10 , translate z = M − M ⋆ z=M-M^\star z = M − M ⋆ , and subtract the constant F x ( M ⋆ ) F_x(M^\star) F x ( M ⋆ ) from the objective. Lemma 11 verifies the boundedness-from-below, weighted-Lipschitz, and quadratic upper-growth conditions in [4 Assumption 3.1] , as well as the far-field quadratic lower-growth condition used in [4 Theorem 3.2] . The coordinate translation only translates the consensus point, and the additive objective constant cancels from its normalized Boltzmann ratio. Applying [4 Theorem 3.2] therefore yields a unique nonlinear strong process on every finite interval, whose law satisfies (20) .
The proof of [4 Theorem 3.2] retains the fourth-moment estimate established in Step 2 of the proof of [4 Theorem 3.1] , which gives the fourth-moment bound in Theorem 5; it also bounds m α [ ρ s ] m_\alpha[\rho_s] m α [ ρ s ] uniformly on [ 0 , S ] [0,S] [ 0 , S ] . Consequently,
sup 0 ≤ s ≤ S E ∥ M s − m α [ ρ s ] ∥ 4 < ∞ . (21) \sup_{0\leq s\leq S}\mathbb{E}\lVert M_s-m_\alpha[\rho_s]\rVert^4<\infty. \tag*{(21)} 0 ≤ s ≤ S sup E ∥ M s − m α [ ρ s ] ∥ 4 < ∞. (21)
For 0 ≤ s < t ≤ S 0\leq s<t\leq S 0 ≤ s < t ≤ S , the same process provides a coupling of ρ s \rho_s ρ s and ρ t \rho_t ρ t . Applying Jensen’s inequality to the drift integral and the Burkholder–Davis–Gundy inequality to the stochastic integral in (11) , together with (21) , gives
W 4 4 ( ρ t , ρ s ) ≤ E ∥ M t − M s ∥ 4 ≤ C S ( ∣ t − s ∣ 4 + ∣ t − s ∣ 2 ) . W_4^4(\rho_t,\rho_s)\leq\mathbb{E}\lVert M_t-M_s\rVert^4\leq C_S\left(|t-s|^4+|t-s|^2\right). W 4 4 ( ρ t , ρ s ) ≤ E ∥ M t − M s ∥ 4 ≤ C S ( ∣ t − s ∣ 4 + ∣ t − s ∣ 2 ) .
Thus W 4 ( ρ t , ρ s ) → 0 W_4(\rho_t,\rho_s)\to0 W 4 ( ρ t , ρ s ) → 0 as t → s t\to s t → s , strengthening the P 2 \mathcal{P}_2 P 2 continuity stated in [4 Theorem 3.2] to ρ ∈ C ( [ 0 , S ] , P 4 ( R D ) ) \rho\in C([0,S],\mathcal{P}_4(\mathbb{R}^D)) ρ ∈ C ([ 0 , S ] , P 4 ( R D )) .
Additional Assumptions and Finite-Accuracy Global Convergence
We apply the isotropic CBO convergence theorem [8 Theorem 3.7] to the mean-field model in § 3 . We retain the fixed x x x , H H H , τ > 0 \tau>0 τ > 0 , Σ ≻ 0 \Sigma\succ0 Σ ≻ 0 and exact, unprojected free energy F x F_x F x used there. We write W 2 ( μ , ν ) W_2(\mu,\nu) W 2 ( μ , ν ) for the 2-Wasserstein distance between probability measures μ \mu μ and ν \nu ν , and δ M \delta_M δ M for the Dirac measure at M M M . The existence of a global minimizer follows from Corollary 10 . For finite-accuracy global convergence, we additionally assume uniqueness and nondegeneracy.
Assumption 12 (Unique nondegenerate global minimizer). The global minimizer M ⋆ M^\star M ⋆ of F x F_x F x is unique and satisfies ∇ 2 F x ( M ⋆ ) ≻ 0 \nabla^2 F_x(M^\star) \succ0 ∇ 2 F x ( M ⋆ ) ≻ 0 .
Assumption 12 is not automatic for general MPC problems; symmetric obstacle avoidance, for example, may yield multiple equivalent minimizers. A different analysis may permit a set-valued conclusion: for a modified CBO model, Fornasier and Sun [7 ] prove exponential concentration toward a set of global minimizers with multiple smooth, convex, and compact components. We assume uniqueness to apply the standard theorem directly. Here and throughout, B r ( M ⋆ ) : = { M ∈ R D : ∥ M − M ⋆ ∥ < r } B_r(M^\star) := \{M \in\mathbb{R}^{D} : \lVert M - M^\star\rVert< r\} B r ( M ⋆ ) := { M ∈ R D : ∥ M − M ⋆ ∥ < r } .
The next lemma is the standard Taylor estimate at a nondegenerate minimizer. We apply it under Assumption 12 to verify the inverse-continuity condition in (Fornasier et al., [8 ] , Definition 3.5).
Lemma 13 (Local quadratic growth near the nondegenerate minimizer). Suppose Assumptions 1 1 1 , 2 2 2 , and 12 12 12 hold. Then there exist constants r > 0 r > 0 r > 0 and μ > 0 \mu> 0 μ > 0 such that for all M ∈ B r ( M ⋆ ) M \in B_r(M^\star) M ∈ B r ( M ⋆ ) ,
F x ( M ) − F x ( M ⋆ ) ≥ μ 2 ∥ M − M ⋆ ∥ 2 . (22) F_x(M) - F_x(M^\star) \ge\frac{\mu}{2}\lVert M - M^\star\rVert^2. \tag*{(22)} F x ( M ) − F x ( M ⋆ ) ≥ 2 μ ∥ M − M ⋆ ∥ 2 . (22) Consequently,
∥ M − M ⋆ ∥ ≤ 2 μ ( F x ( M ) − F x ( M ⋆ ) ) 1 / 2 (23) \lVert M - M^\star\rVert\le\sqrt{\frac{2}{\mu}}\left(F_x(M) - F_x(M^\star)\right)^{1/2} \tag*{(23)} ∥ M − M ⋆ ∥ ≤ μ 2 ( F x ( M ) − F x ( M ⋆ ) ) 1/2 (23) holds on B r ( M ⋆ ) B_r(M^\star) B r ( M ⋆ ) .
Proof. By Assumption 12, ∇ 2 F x ( M ⋆ ) ≻ 0 \nabla^2 F_x(M^\star) \succ0 ∇ 2 F x ( M ⋆ ) ≻ 0 , so there exists μ > 0 \mu> 0 μ > 0 such that ∇ 2 F x ( M ⋆ ) ⪰ 2 μ I D \nabla^2 F_x(M^\star) \succeq2\mu I_D ∇ 2 F x ( M ⋆ ) ⪰ 2 μ I D . Since F x ∈ C ∞ F_x \in C^\infty F x ∈ C ∞ by Lemma 7, ∇ 2 F x \nabla^2 F_x ∇ 2 F x is continuous, and for r > 0 r > 0 r > 0 sufficiently small, ∇ 2 F x ( M ) ⪰ μ I D \nabla^2 F_x(M) \succeq\mu I_D ∇ 2 F x ( M ) ⪰ μ I D for all M ∈ B r ( M ⋆ ) M \in B_r(M^\star) M ∈ B r ( M ⋆ ) . Since F x F_x F x is smooth and M ⋆ M^\star M ⋆ is a global minimizer, ∇ F x ( M ⋆ ) = 0 \nabla F_x(M^\star) = 0 ∇ F x ( M ⋆ ) = 0 . Taylor’s theorem then gives, for any M ∈ B r ( M ⋆ ) M \in B_r(M^\star) M ∈ B r ( M ⋆ ) , some θ ∈ ( 0 , 1 ) \theta\in(0,1) θ ∈ ( 0 , 1 ) , and M θ : = M ⋆ + θ ( M − M ⋆ ) M_\theta:= M^\star+ \theta(M - M^\star) M θ := M ⋆ + θ ( M − M ⋆ ) ,
F x ( M ) − F x ( M ⋆ ) = ∇ F x ( M ⋆ ) ⋅ ( M − M ⋆ ) + 1 2 ( M − M ⋆ ) T ∇ 2 F x ( M θ ) ( M − M ⋆ ) ≥ μ 2 ∥ M − M ⋆ ∥ 2 , \begin{aligned}
F_x(M) - F_x(M^\star) &= \nabla F_x(M^\star) \cdot(M - M^\star) + \frac{1}{2}(M - M^\star)^{\mathsf{T}}\nabla^2 F_x(M_\theta)(M - M^\star) \\
&\ge\frac{\mu}{2}\lVert M - M^\star\rVert^2,
\end{aligned} F x ( M ) − F x ( M ⋆ ) = ∇ F x ( M ⋆ ) ⋅ ( M − M ⋆ ) + 2 1 ( M − M ⋆ ) T ∇ 2 F x ( M θ ) ( M − M ⋆ ) ≥ 2 μ ∥ M − M ⋆ ∥ 2 ,
establishing (22) . Rearranging gives (23) . ■
Lemma 14 (CBO objective conditions). Under Assumptions 1 1 1 , 2 2 2 , and 12 12 12 , there exist constants c i c , R 0 , E ∞ > 0 c_{\mathrm{ic}}, R_0, E_\infty> 0 c ic , R 0 , E ∞ > 0 such that
∥ M − M ⋆ ∥ ≤ 1 c i c ( F x ( M ) − F x ( M ⋆ ) ) 1 / 2 , M ∈ B R 0 ( M ⋆ ) , (24) \lVert M - M^\star\rVert\le\frac{1}{c_{\mathrm{ic}}}\left(F_x(M) - F_x(M^\star)\right)^{1/2}, \qquad M \in B_{R_0}(M^\star), \tag*{(24)} ∥ M − M ⋆ ∥ ≤ c ic 1 ( F x ( M ) − F x ( M ⋆ ) ) 1/2 , M ∈ B R 0 ( M ⋆ ) , (24) and
F x ( M ) − F x ( M ⋆ ) > E ∞ , M ∈ B R 0 ( M ⋆ ) c . (25) F_x(M) - F_x(M^\star) > E_\infty, \qquad M \in B_{R_0}(M^\star)^c. \tag*{(25)} F x ( M ) − F x ( M ⋆ ) > E ∞ , M ∈ B R 0 ( M ⋆ ) c . (25) Proof. By Lemma 13, there exist r > 0 r > 0 r > 0 and μ > 0 \mu> 0 μ > 0 such that on B r ( M ⋆ ) B_r(M^\star) B r ( M ⋆ ) ,
∥ M − M ⋆ ∥ ≤ 2 μ ( F x ( M ) − F x ( M ⋆ ) ) 1 / 2 . \lVert M - M^\star\rVert\le\sqrt{\frac{2}{\mu}}\left(F_x(M) - F_x(M^\star)\right)^{1/2}. ∥ M − M ⋆ ∥ ≤ μ 2 ( F x ( M ) − F x ( M ⋆ ) ) 1/2 . e n d s m i d = 1 {ends_mid=1} e n d s m i d = 1
By Lemma 9 , F x ( M ) → ∞ F_x(M) \to\infty F x ( M ) → ∞ as ∥ M ∥ → ∞ \lVert M\rVert\to\infty ∥ M ∥ → ∞ . Choosing R 0 > r R_0>r R 0 > r large enough gives
inf M ∈ B R 0 ( M ⋆ ) c ( F x ( M ) − F x ( M ⋆ ) ) > 0. \inf_{M\in B_{R_0}(M^\star)^c}\left(F_x(M)-F_x(M^\star)\right)>0. M ∈ B R 0 ( M ⋆ ) c inf ( F x ( M ) − F x ( M ⋆ ) ) > 0.
Setting
E ∞ : = 1 2 inf M ∈ B R 0 ( M ⋆ ) c ( F x ( M ) − F x ( M ⋆ ) ) > 0 E_\infty:=\frac{1}{2}\inf_{M\in B_{R_0}(M^\star)^c}\left(F_x(M)-F_x(M^\star)\right)>0 E ∞ := 2 1 M ∈ B R 0 ( M ⋆ ) c inf ( F x ( M ) − F x ( M ⋆ ) ) > 0
yields (25) . For the annular region
A : = { M ∈ R D : r ≤ ∥ M − M ⋆ ∥ ≤ R 0 } , A:=\left\{M\in\mathbb{R}^D:r\leq\lVert M-M^\star\rVert\leq R_0\right\}, A := { M ∈ R D : r ≤ ∥ M − M ⋆ ∥ ≤ R 0 } ,
A A A is compact, F x F_x F x is continuous, and M ⋆ ∉ A M^\star\notin A M ⋆ ∈ / A , so
δ A : = min M ∈ A ( F x ( M ) − F x ( M ⋆ ) ) > 0. \delta_A:=\min_{M\in A}\left(F_x(M)-F_x(M^\star)\right)>0. δ A := M ∈ A min ( F x ( M ) − F x ( M ⋆ ) ) > 0.
For M ∈ A M\in A M ∈ A ,
∥ M − M ⋆ ∥ ≤ R 0 ≤ R 0 δ A ( F x ( M ) − F x ( M ⋆ ) ) 1 / 2 . \lVert M-M^\star\rVert\leq R_0\leq\frac{R_0}{\sqrt{\delta_A}}\left(F_x(M)-F_x(M^\star)\right)^{1/2}. ∥ M − M ⋆ ∥ ≤ R 0 ≤ δ A R 0 ( F x ( M ) − F x ( M ⋆ ) ) 1/2 .
Setting
c i c : = min { μ 2 , δ A R 0 } c_{\mathrm{ic}}:=\min\left\{\sqrt{\frac{\mu}{2}},\frac{\sqrt{\delta_A}}{R_0}\right\} c ic := min { 2 μ , R 0 δ A }
gives (24) uniformly on B R 0 ( M ⋆ ) B_{R_0}(M^\star) B R 0 ( M ⋆ ) .
Global convergence also requires an initial support condition and a bound on the diffusion strength.
Assumption 15 (Initial distribution and CBO parameters). The initial distribution ρ 0 ∈ P 4 ( R D ) \rho_0\in\mathcal{P}_4(\mathbb{R}^D) ρ 0 ∈ P 4 ( R D ) satisfies ρ 0 ( B r ( M ⋆ ) ) > 0 \rho_0(B_r(M^\star))>0 ρ 0 ( B r ( M ⋆ )) > 0 for every r > 0 r>0 r > 0 . Furthermore, λ , σ c b o > 0 \lambda,\sigma_{\mathrm{cbo}}>0 λ , σ cbo > 0 satisfy 2 λ − D σ c b o 2 > 0 2\lambda-D\sigma_{\mathrm{cbo}}^2>0 2 λ − D σ cbo 2 > 0 .
The condition ρ 0 ( B r ( M ⋆ ) ) > 0 \rho_0(B_r(M^\star))>0 ρ 0 ( B r ( M ⋆ )) > 0 for every r > 0 r>0 r > 0 is equivalent to M ⋆ ∈ supp ρ 0 M^\star\in\operatorname{supp}\rho_0 M ⋆ ∈ supp ρ 0 . That is, ρ 0 \rho_0 ρ 0 assigns positive probability to every neighborhood of the global minimizer.
We analyze finite-accuracy convergence using the energy functional
V ( ρ ) : = 1 2 ∫ R D ∥ M − M ⋆ ∥ 2 ρ ( d M ) , \mathcal{V}(\rho):=\frac{1}{2}\int_{\mathbb{R}^D}\lVert M-M^\star\rVert^2\rho(dM), V ( ρ ) := 2 1 ∫ R D ∥ M − M ⋆ ∥ 2 ρ ( d M ) ,
which measures the mean squared distance from the unique global minimizer M ⋆ M^\star M ⋆ .
Theorem 16 (Finite-accuracy global convergence; Fornasier et al., [8 ] , Theorem 3.7). Suppose Assumptions 1, 2, 12, and 15 hold. For any ε ∈ ( 0 , V ( ρ 0 ) ) \varepsilon\in(0,\mathcal{V}(\rho_0)) ε ∈ ( 0 , V ( ρ 0 )) and any ϑ ∈ ( 0 , 1 ) \vartheta\in(0,1) ϑ ∈ ( 0 , 1 ) , define
S ⋆ : = 1 ( 1 − ϑ ) ( 2 λ − D σ c b o 2 ) log ( V ( ρ 0 ) ε ) . (26) S^\star:=\frac{1}{(1-\vartheta)(2\lambda-D\sigma_{\mathrm{cbo}}^2)}\log\left(\frac{\mathcal{V}(\rho_0)}{\varepsilon}\right). \tag*{(26)} S ⋆ := ( 1 − ϑ ) ( 2 λ − D σ cbo 2 ) 1 log ( ε V ( ρ 0 ) ) . (26)
There exists α 0 > 0 \alpha_0 > 0 α 0 > 0 , depending in particular on ϵ \epsilon ϵ , ϑ \vartheta ϑ , D D D , λ \lambda λ , σ c b o \sigma_{\mathrm{cbo}} σ cbo , the initial law ρ 0 \rho_0 ρ 0 , and the objective constants and local sublevel geometry of F x F_x F x , such that for every α > α 0 \alpha> \alpha_0 α > α 0 , the law ρ s : = Law ( M s ) \rho_s := \operatorname{Law}(M_s) ρ s := Law ( M s ) of the unique nonlinear process M M M from Theorem 5 , initialized with law ρ 0 \rho_0 ρ 0 , satisfies
min 0 ≤ s ≤ S ⋆ V ( ρ s ) ≤ ϵ . \min_{0 \le s \le S^\star} \mathcal{V}(\rho_s) \le\epsilon. 0 ≤ s ≤ S ⋆ min V ( ρ s ) ≤ ϵ .
Moreover, until V ( ρ s ) \mathcal{V}(\rho_s) V ( ρ s ) first reaches ϵ \epsilon ϵ ,
V ( ρ s ) ≤ V ( ρ 0 ) exp ( − ( 1 − ϑ ) ( 2 λ − D σ c b o 2 ) s ) . (27) \mathcal{V}(\rho_s) \le\mathcal{V}(\rho_0)\exp\left(-(1-\vartheta)(2\lambda-D\sigma_{\mathrm{cbo}}^2)s\right). \tag*{(27)} V ( ρ s ) ≤ V ( ρ 0 ) exp ( − ( 1 − ϑ ) ( 2 λ − D σ cbo 2 ) s ) . (27)
In particular, there exists s ϵ ∈ [ 0 , S ⋆ ] s_\epsilon\in[0,S^\star] s ϵ ∈ [ 0 , S ⋆ ] such that W 2 ( ρ s ϵ , δ M ⋆ ) ≤ 2 ϵ W_2(\rho_{s_\epsilon},\delta_{M^\star}) \le\sqrt{2\epsilon} W 2 ( ρ s ϵ , δ M ⋆ ) ≤ 2 ϵ .
Proof. Lemmas 6 and 7 show that F x F_x F x is a continuous real-valued objective on R D \mathbb{R}^D R D . Lemma 9 and Corollary 10 show that it attains its global minimum. Hence condition A1 in [8 Definition 3.5] holds.
Under Assumption 12, Lemma 14 gives (24) and (25) . These are the inverse-continuity and far-field parts of condition A2 in [8 Definition 3.5] , with exponent ν = 1 / 2 \nu= 1/2 ν = 1/2 .
Assumption 15 gives ρ 0 ∈ P 4 ( R D ) \rho_0 \in\mathcal{P}_4(\mathbb{R}^D) ρ 0 ∈ P 4 ( R D ) , M ⋆ ∈ supp ρ 0 M^\star\in\operatorname{supp}\rho_0 M ⋆ ∈ supp ρ 0 , and 2 λ > D σ c b o 2 2\lambda> D\sigma_{\mathrm{cbo}}^2 2 λ > D σ cbo 2 . Theorem 5 supplies the unique process and its law ρ ∈ C ( [ 0 , S ⋆ ] , P 4 ( R D ) ) \rho\in C([0,S^\star],\mathcal{P}_4(\mathbb{R}^D)) ρ ∈ C ([ 0 , S ⋆ ] , P 4 ( R D )) . Equation (20) shows that this law weakly solves (19) .
The dynamics (11) coincide with the standard isotropic CBO dynamics of [8 Theorem 3.7] when the cutoff function is identically equal to one. The probability-mass estimate [8 Proposition 4.6] , together with M ⋆ ∈ supp ρ 0 M^\star\in\operatorname{supp}\rho_0 M ⋆ ∈ supp ρ 0 and finite-time boundedness of m α [ ρ s ] m_\alpha[\rho_s] m α [ ρ s ] , gives, for each fixed r > 0 r > 0 r > 0 and S > 0 S > 0 S > 0 ,
ρ s ( B r ( M ⋆ ) ) ≥ ( ∫ R D ϕ r ( M ) ρ 0 ( d M ) ) e − p r , S s , 0 ≤ s ≤ S . \rho_s(B_r(M^\star)) \ge\left(\int_{\mathbb{R}^D}\phi_r(M)\rho_0(dM)\right)e^{-p_{r,S}s}, \qquad0 \le s \le S. ρ s ( B r ( M ⋆ )) ≥ ( ∫ R D ϕ r ( M ) ρ 0 ( d M ) ) e − p r , S s , 0 ≤ s ≤ S .
where ϕ r \phi_r ϕ r is the compactly supported mollifier in the cited proposition and p r , S p_{r,S} p r , S is a finite constant whose dependence includes the finite-time consensus bound. Since ϕ r \phi_r ϕ r is bounded below by some c r > 0 c_r > 0 c r > 0 on B r / 2 ( M ⋆ ) B_{r/2}(M^\star) B r /2 ( M ⋆ ) ,
∫ R D ϕ r ( M ) ρ 0 ( d M ) ≥ c r ρ 0 ( B r / 2 ( M ⋆ ) ) > 0. \int_{\mathbb{R}^D}\phi_r(M)\rho_0(dM) \ge c_r\rho_0(B_{r/2}(M^\star)) > 0. ∫ R D ϕ r ( M ) ρ 0 ( d M ) ≥ c r ρ 0 ( B r /2 ( M ⋆ )) > 0.
Hence inf 0 ≤ s ≤ S ρ s ( B r ( M ⋆ ) ) > 0 \inf_{0 \le s \le S}\rho_s(B_r(M^\star)) > 0 inf 0 ≤ s ≤ S ρ s ( B r ( M ⋆ )) > 0 . This lower bound controls the denominator in the quantitative Laplace estimate underlying [8 Theorem 3.7] .
Applying [8 Theorem 3.7] with dimension d = D d = D d = D , objective E = F x \mathcal{E} = F_x E = F x , and diffusion coefficient σ = σ c b o \sigma= \sigma_{\mathrm{cbo}} σ = σ cbo gives the horizon (26) , the exponential estimate (27) , and a time s ϵ ≤ S ⋆ s_\epsilon\le S^\star s ϵ ≤ S ⋆ at which V ( ρ s ϵ ) ≤ ϵ \mathcal{V}(\rho_{s_\epsilon}) \le\epsilon V ( ρ s ϵ ) ≤ ϵ . Since W 2 2 ( ρ s , δ M ⋆ ) = 2 V ( ρ s ) W_2^2(\rho_s,\delta_{M^\star}) = 2\mathcal{V}(\rho_s) W 2 2 ( ρ s , δ M ⋆ ) = 2 V ( ρ s ) , the final bound follows.
The theorem establishes concentration near the global minimizer of F x F_x F x . Under the stated assumptions, any prescribed tolerance ϵ \epsilon ϵ can be reached by choosing α \alpha α sufficiently large. At some CBO pseudo-time s ϵ ≤ S ⋆ s_\epsilon\le S^\star s ϵ ≤ S ⋆ , the root mean squared distance from M ⋆ M^\star M ⋆ is at most 2 ϵ \sqrt{2\epsilon} 2 ϵ , with an exponential bound on the mean squared distance up to this first attainment. The result applies to a fixed open-loop problem with exact, unprojected free energy and mean-field dynamics. Finite-particle and finite-rollout convergence, input projection, warm starts, and closed-loop stability remain open.
Algorithms and Numerical Details
This appendix documents the three comparison controllers. Subsection B.1 specifies MPPI, MS-MPPI, and CBO-MPC, including their output and warm-start rules.
Baseline Controller Algorithms
MPPI
MPPI maintains one nominal sequence and applies the weighted-mean update in (5) . The updated sequence supplies the first input and is shifted for the next control period.
Algorithm 2: MPPI: one control period
Require: State x x x ; initialized or warm-started sequence M ∈ R m H M \in\mathbb{R}^{mH} M ∈ R m H ; L L L , τ > 0 \tau> 0 τ > 0 , Σ ≻ 0 \Sigma\succ0 Σ ≻ 0
Ensure: Applied input and next-period nominal sequence
Draw ϵ j ∼ N ( 0 , Σ ) \epsilon_j \sim\mathcal{N}(0,\Sigma) ϵ j ∼ N ( 0 , Σ ) and set U j = M + ϵ j U_j = M + \epsilon_j U j = M + ϵ j , j = 1 , … , L j = 1,\ldots,L j = 1 , … , L
Roll out from x x x and evaluate S j = J ˉ x ( U j ) S_j = \bar{J}_x(U_j) S j = J ˉ x ( U j ) for all j j j in parallel
Compute Z ^ = L − 1 ∑ j e − S j / τ \hat{Z} = L^{-1}\sum_j e^{-S_j/\tau} Z ^ = L − 1 ∑ j e − S j / τ and M + M^{+} M + by (5) , using the zero-weight safeguard if needed
Apply [ M + ] 0 [M^{+}]_0 [ M + ] 0 and set M n e x t = Shift ( M + ) M^{\mathrm{next}} = \operatorname{Shift}(M^{+}) M next = Shift ( M + )
MS-MPPI
MS-MPPI maintains N N N persistent centers M i k ∈ R D M_i^k \in\mathbb{R}^{D} M i k ∈ R D . At each round, it draws U i , j k = M i k + ϵ i , j k U_{i,j}^k = M_i^k + \epsilon_{i,j}^k U i , j k = M i k + ϵ i , j k , with ϵ i , j k ∼ N ( 0 , Σ ) \epsilon_{i,j}^k \sim\mathcal{N}(0,\Sigma) ϵ i , j k ∼ N ( 0 , Σ ) , and evaluates S i , j k = J ˉ x ( U i , j k ) S_{i,j}^k = \bar{J}_x(U_{i,j}^k) S i , j k = J ˉ x ( U i , j k ) . Each local batch gives
Z ^ i k = 1 L ∑ j = 1 L e − S i , j k / τ , F ^ i k = − τ log Z ^ i k , w i , j k = e − S i , j k / τ L Z ^ i k . (28) \hat{Z}_i^k = \frac{1}{L}\sum_{j=1}^{L} e^{-S_{i,j}^k/\tau}, \qquad\hat{F}_i^k = -\tau\log\hat{Z}_i^k, \qquad w_{i,j}^k = \frac{e^{-S_{i,j}^k/\tau}}{L\hat{Z}_i^k}. \tag*{(28)} Z ^ i k = L 1 j = 1 ∑ L e − S i , j k / τ , F ^ i k = − τ log Z ^ i k , w i , j k = L Z ^ i k e − S i , j k / τ . (28)
For the first K − 1 K-1 K − 1 rounds, every center follows its own MPPI update,
M i k + 1 = ∑ j = 1 L w i , j k U i , j k , k = 0 , … , K − 2. (29) M_i^{k+1} = \sum_{j=1}^{L} w_{i,j}^k U_{i,j}^k, \qquad k = 0,\ldots,K-2. \tag*{(29)} M i k + 1 = j = 1 ∑ L w i , j k U i , j k , k = 0 , … , K − 2. (29)
The final round selects i ⋆ ∈ arg min i F ^ i K − 1 i^\star\in\arg\min_i \hat{F}_i^{K-1} i ⋆ ∈ arg min i F ^ i K − 1 and returns U o u t = ∑ j w i ⋆ , j K − 1 U i ⋆ , j K − 1 U_{\mathrm{out}} = \sum_j w_{i^\star,j}^{K-1}U_{i^\star,j}^{K-1} U out = ∑ j w i ⋆ , j K − 1 U i ⋆ , j K − 1 , using the same selection and output rule as CBO-MPPI. Each final evaluated center is shifted and jittered independently for the next control period, rather than replacing the population by the selected output. An all-infeasible local batch has F ^ i k = + ∞ \hat{F}_i^k = +\infty F ^ i k = + ∞ and uses its projected center as its local update or output.
CBO-MPC
CBO-MPC treats each particle U i k ∈ R D U_i^k \in\mathbb{R}^{D} U i k ∈ R D as a candidate control sequence and evaluates S i k = J ˉ x ( U i k ) S_i^k = \bar{J}_x(U_i^k) S i k = J ˉ x ( U i k ) directly [3 , 16 ] . For α > 0 \alpha> 0 α > 0 , its cost-based weights and consensus are
ν i k = e − α S i k ∑ r = 1 N e − α S r k , U α k = ∑ i = 1 N ν i k U i k . (30) \nu_i^k = \frac{e^{-\alpha S_i^k}}{\sum_{r=1}^{N} e^{-\alpha S_r^k}}, \qquad U_\alpha^k = \sum_{i=1}^{N} \nu_i^k U_i^k. \tag*{(30)} ν i k = ∑ r = 1 N e − α S r k e − α S i k , U α k = i = 1 ∑ N ν i k U i k . (30)
Algorithm 3: MS-MPPI: one control period
Require: State x x x ; initialized or warm-started centers M i 0 ∈ R m H M_i^0 \in\mathbb{R}^{mH} M i 0 ∈ R m H , i = 1 , … , N i=1,\ldots,N i = 1 , … , N ; K , L , τ > 0 K,L,\tau>0 K , L , τ > 0 , Σ ≻ 0 \Sigma\succ0 Σ ≻ 0 ; σ j i t , B j i t \sigma_{\mathrm{jit}},B_{\mathrm{jit}} σ jit , B jit
Ensure: Applied input and next-period centers
for k = 0 , … , K − 1 k=0,\ldots,K-1 k = 0 , … , K − 1 do
for all centers i = 1 , … , N i=1,\ldots,N i = 1 , … , N in parallel do
Draw ϵ i , j k ∼ N ( 0 , Σ ) \epsilon_{i,j}^k \sim\mathcal{N}(0,\Sigma) ϵ i , j k ∼ N ( 0 , Σ ) ; set U i , j k = M i k + ϵ i , j k U_{i,j}^k=M_i^k+\epsilon_{i,j}^k U i , j k = M i k + ϵ i , j k and evaluate S i , j k = J ˉ x ( U i , j k ) S_{i,j}^k=\bar{J}_x(U_{i,j}^k) S i , j k = J ˉ x ( U i , j k ) from x x x
Compute F ^ i k \widehat{F}_i^k F i k and the local weighted mean from (28) , with the zero-weight safeguard
if k < K − 1 k<K-1 k < K − 1 then
Update M i k + 1 M_i^{k+1} M i k + 1 by (29)
end if
end for
end for
Select i ⋆ ∈ arg min i F ^ i K − 1 i^\star\in\arg\min_i \widehat{F}_i^{K-1} i ⋆ ∈ arg min i F i K − 1 ; reuse its final local mean as U o u t U_{\mathrm{out}} U out and apply [ U o u t ] 0 [U_{\mathrm{out}}]_0 [ U out ] 0
for all i = 1 , … , N i=1,\ldots,N i = 1 , … , N do
Draw A i A_i A i with independent N ( 0 , 1 ) \mathcal{N}(0,1) N ( 0 , 1 ) entries; set ζ i = σ j i t B j i t A i \zeta_i=\sigma_{\mathrm{jit}}B_{\mathrm{jit}}A_i ζ i = σ jit B jit A i
Set M i 0 , n e x t = Shift ( M i K − 1 ) + ζ i M_i^{0,\mathrm{next}}=\operatorname{Shift}(M_i^{K-1})+\zeta_i M i 0 , next = Shift ( M i K − 1 ) + ζ i
end for
After each of the first K − 1 K-1 K − 1 evaluations, the particles move according to
U i k + 1 = U i k − λ Δ s ( U i k − U α k ) + σ c b o ∥ U i k − U α k ∥ ξ i k Δ s , ξ i k ∼ N ( 0 , I D ) . (31) U_i^{k+1}=U_i^k-\lambda\Delta s(U_i^k-U_\alpha^k)+\sigma_{\mathrm{cbo}}\lVert U_i^k-U_\alpha^k\rVert\xi_i^k\sqrt{\Delta s},\qquad\xi_i^k\sim\mathcal{N}(0,I_D). \tag*{(31)} U i k + 1 = U i k − λ Δ s ( U i k − U α k ) + σ cbo ∥ U i k − U α k ∥ ξ i k Δ s , ξ i k ∼ N ( 0 , I D ) . (31)
Here λ , σ c b o , Δ s > 0 \lambda,\sigma_{\mathrm{cbo}},\Delta s>0 λ , σ cbo , Δ s > 0 have the same roles as in (9) . The final output is the minimum-cost particle: i ⋆ ∈ arg min i S i K − 1 i^\star\in\arg\min_i S_i^{K-1} i ⋆ ∈ arg min i S i K − 1 and U o u t = U i ⋆ K − 1 U_{\mathrm{out}}=U_{i^\star}^{K-1} U out = U i ⋆ K − 1 . There is no local MPPI batch or weighted-mean output. In the numerical implementation, an all-infeasible population uses the mean of its projected particles for consensus and output.
Algorithm 4: CBO-MPC: one control period
Require: State x x x ; initialized or warm-started particles U i 0 ∈ R m H U_i^0 \in\mathbb{R}^{mH} U i 0 ∈ R m H , i = 1 , … , N i = 1,\ldots,N i = 1 , … , N ; K K K , α \alpha α , λ \lambda λ , σ c b o \sigma_{\mathrm{cbo}} σ cbo , Δ s > 0 \Delta s > 0 Δ s > 0 ; σ j i t \sigma_{\mathrm{jit}} σ jit , B j i t B_{\mathrm{jit}} B jit
Ensure: Applied input and next-period particles
for k = 0 , … , K − 1 k = 0,\ldots,K-1 k = 0 , … , K − 1 do
Roll out every U i k U_i^k U i k from x x x and evaluate S i k = J ˉ x ( U i k ) S_i^k = \bar{J}_x(U_i^k) S i k = J ˉ x ( U i k ) in parallel
Compute U α k U_\alpha^k U α k by (30) , using the all-infeasible safeguard if needed
if k < K − 1 k < K-1 k < K − 1 then
Draw independent ξ i k ∼ N ( 0 , I D ) \xi_i^k \sim\mathcal{N}(0,I_D) ξ i k ∼ N ( 0 , I D ) and update particles by (31)
end if
end for
Select i ⋆ ∈ arg min i S i K − 1 i^\star\in\arg\min_i S_i^{K-1} i ⋆ ∈ arg min i S i K − 1 and set U o u t = U i ⋆ K − 1 U_{\mathrm{out}} = U_{i^\star}^{K-1} U out = U i ⋆ K − 1 , using the all-infeasible safeguard if needed; apply [ U o u t ] 0 [U_{\mathrm{out}}]_0 [ U out ] 0
for all i = 1 , … , N i = 1,\ldots,N i = 1 , … , N do
Draw A i A_i A i with independent N ( 0 , 1 ) \mathcal{N}(0,1) N ( 0 , 1 ) entries; set ζ i = σ j i t B j i t A i \zeta_i = \sigma_{\mathrm{jit}}B_{\mathrm{jit}}A_i ζ i = σ jit B jit A i
Set U i 0 , n e x t = Shift ( U i K − 1 ) + ζ i U_i^{0,\mathrm{next}} = \operatorname{Shift}(U_i^{K-1}) + \zeta_i U i 0 , next = Shift ( U i K − 1 ) + ζ i
end for
Complete Numerical Results
We give the common numerical protocol, task specifications, complete results, and parameter-sensitivity analysis.
Common Numerical Protocol
Common settings. Within each task, all four controllers use the same dynamics, trajectory cost, constraints, control period, prediction horizon H H H , closed-loop evaluation window T T T , input set, and total rollout budget L t o t L_{\mathrm{tot}} L tot per control step. These quantities vary across tasks and are specified in Appendices C.2–C.5. Preliminary timing experiments set each rollout budget so that CBO-MPPI’s median run-level control-cycle 95th percentile was below the task’s control period.
MPPI-based controller settings. MPPI, MS-MPPI, and CBO-MPPI use the same task-dependent temperature and local perturbation covariance. Preliminary free-energy landscapes determine the low- and high-temperature regimes, and preliminary small-scale experiments determine the reference MPPI and CBO-MPPI parameters. These task-dependent regimes give τ = 3000 \tau= 3000 τ = 3000 for Many Obstacles and τ = 300 \tau= 300 τ = 300 for Navigation. MPPI uses one nominal sequence and L = L t o t L = L_{\mathrm{tot}} L = L tot rollouts. MS-MPPI and CBO-MPPI share ( N , L , K ) (N,L,K) ( N , L , K ) , the initial-center law, local perturbations, center-selection and MPPI weighted-output rule, input projection, and warm start; they differ only in the center update, using (29) and (9), respectively.
CBO-MPPI uses λ = 1 \lambda= 1 λ = 1 , no gradient assistance, and the reference noise-to-drift ratio
κ : = D σ c b o 2 2 λ = 0.5 , σ c b o = 2 λ κ D . \kappa:= \frac{D\sigma_{\mathrm{cbo}}^2}{2\lambda} = 0.5,\qquad\sigma_{\mathrm{cbo}} = \sqrt{\frac{2\lambda\kappa}{D}}. κ := 2 λ D σ cbo 2 = 0.5 , σ cbo = D 2 λκ .
The reference α \alpha α , Δ s \Delta s Δ s , covariance, initialization, warm-start jitter, and allocation vary across tasks and are given in the task sections.
CBO-MPC settings. CBO-MPC inherits α \alpha α , λ \lambda λ , κ \kappa κ , σ c b o \sigma_{\mathrm{cbo}} σ cbo , Δ s \Delta_s Δ s , the initialization law, warm start, input projection, and K K K from CBO-MPPI. It sets N C B O − M P C = N C B O − M P P I L C B O − M P P I N_{\mathrm{CBO-MPC}} = N_{\mathrm{CBO-MPPI}}L_{\mathrm{CBO-MPPI}} N CBO − MPC = N CBO − MPPI L CBO − MPPI to match the common rollout budget. CBO-MPC forms its cost-weighted consensus by (30) , whereas CBO-MPPI forms its free-energy-weighted consensus by (8) . The sensitivity study varies only α \alpha α or κ \kappa κ for CBO-MPPI; the horizon study varies H H H and recomputes σ c b o \sigma_{\mathrm{cbo}} σ cbo to preserve κ \kappa κ . All other quantities retain the corresponding task reference values.
Temporal correlation. This paragraph specifies the autoregressive correlation construction used for the sampling covariance Σ \Sigma Σ in (3) and its numerical counterparts: the local Gaussian perturbations in Algorithms 1–3, the initial center or particle populations in Algorithms 1, 3, and 4, and the post-hoc evaluation perturbations in (13) . It does not apply to the isotropic CBO diffusion vectors ξ i k \xi_i^k ξ i k in (9) and (31) , or to the smooth warm-start jitter defined below. For controller sampling and initialization, write ρ \rho ρ for the temporal correlation and σ \sigma σ for the marginal standard deviation, and view an H H H -step perturbation as an array ϵ ∈ R H × m \epsilon\in\mathbb{R}^{H \times m} ϵ ∈ R H × m . Each channel c c c is a stationary first-order autoregressive sequence,
ϵ 0 , c = σ η 0 , c , ϵ s , c = ρ ϵ s − 1 , c + σ 1 − ρ 2 η s , c , s = 1 , … , H − 1 , (32) \epsilon_{0,c} = \sigma\eta_{0,c}, \qquad\epsilon_{s,c} = \rho\epsilon_{s-1,c} + \sigma\sqrt{1-\rho^2}\,\eta_{s,c}, \qquad s = 1,\ldots,H-1, \tag*{(32)} ϵ 0 , c = σ η 0 , c , ϵ s , c = ρ ϵ s − 1 , c + σ 1 − ρ 2 η s , c , s = 1 , … , H − 1 , (32)
where the η s , c \eta_{s,c} η s , c are independent standard Gaussian variables. Every time step therefore has marginal standard deviation σ \sigma σ and Cov ( ϵ s , c , ϵ t , c ) = σ 2 ρ ∣ s − t ∣ \operatorname{Cov}(\epsilon_{s,c},\epsilon_{t,c}) = \sigma^2\rho^{|s-t|} Cov ( ϵ s , c , ϵ t , c ) = σ 2 ρ ∣ s − t ∣ , so the sampling covariance Σ \Sigma Σ of § 2 is σ 2 R ρ ⊗ I m \sigma^2 R_\rho\otimes I_m σ 2 R ρ ⊗ I m with ( R ρ ) s t = ρ ∣ s − t ∣ (R_\rho)_{st} = \rho^{|s-t|} ( R ρ ) s t = ρ ∣ s − t ∣ . We use ρ = 0.9 \rho= 0.9 ρ = 0.9 for Navigation, Many Obstacles, and Pendulum. The 10-Hz Planar Arm discretization uses ρ = 0.81 = 0.9 2 \rho= 0.81 = 0.9^2 ρ = 0.81 = 0. 9 2 , preserving the correlation over the same physical time separation. For the post-hoc evaluation in (13) , we apply (32) over the full T T T -step evaluation window. The task sections give the corresponding σ \sigma σ for each use.
Warm starts. Algorithms 1, 3, and 4 warm-start each center or particle by shifting it and adding the smooth jitter ζ i \zeta_i ζ i defined below. We use R j i t = min ( 4 , H ) R_{\mathrm{jit}} = \min(4,H) R jit = min ( 4 , H ) cosine modes; each center or particle is viewed as an H × m H \times m H × m array, and A i ∈ R R j i t × m A_i \in\mathbb{R}^{R_{\mathrm{jit}} \times m} A i ∈ R R jit × m contains independent standard Gaussian entries:
ζ i = σ j i t B j i t A i , B ~ s q = cos ( π t s q ) , t s = s + 1 2 H , s = 0 , … , H − 1 , q = 0 , … , R j i t − 1 , B j i t [ s , : ] = B ~ [ s , : ] / ∥ B ~ [ s , : ] ∥ 2 . (33) \begin{aligned}
\zeta_i &= \sigma_{\mathrm{jit}} B_{\mathrm{jit}} A_i, \qquad\widetilde{B}_{sq} = \cos(\pi t_s q), \qquad t_s = \frac{s+\frac{1}{2}}{H},\\
s &= 0,\ldots,H-1,\qquad q = 0,\ldots,R_{\mathrm{jit}}-1,\qquad B_{\mathrm{jit}}[s,:] = \widetilde{B}[s,:]/\lVert\widetilde{B}[s,:]\rVert_2. \tag*{(33)}
\end{aligned} ζ i s = σ jit B jit A i , B s q = cos ( π t s q ) , t s = H s + 2 1 , = 0 , … , H − 1 , q = 0 , … , R jit − 1 , B jit [ s , : ] = B [ s , : ] / ∥ B [ s , : ] ∥ 2 . (33)
Because each row of B j i t B_{\mathrm{jit}} B jit is a unit vector, every time step receives jitter of marginal standard deviation σ j i t \sigma_{\mathrm{jit}} σ jit . The basis restricts the jitter to the lowest-frequency modes, so it varies smoothly along the horizon. For the shift operations in Algorithms 1, 3, and 4, the implementation appends zero.
Closed-loop evaluation. To evaluate F C L F_{\mathrm{CL}} F CL in (13) , we hold the realized input sequence fixed and roll out each entire perturbed sequence from x 0 x_0 x 0 , without rerunning feedback control. Its cost and admissible set use the initial state x 0 x_0 x 0 and the full T T T -step evaluation window. Evaluation perturbations have the controller’s marginal standard deviation and temporal correlation, extended from H H H to T T T steps. The same antithetic perturbations are reused across methods and trials within each task, with L e = 32768 L_e = 32768 L e = 32768 except for Many Obstacles, where L e = 8192 L_e = 8192 L e = 8192 . The input-increment penalties specified in each task are included in J C L J_{\mathrm{CL}} J CL ; state-dependent stage terms use x k + 1 x_{k+1} x k + 1 , which gives the convention of § 2 after composition with f f f .
Success criteria are given in each task section. In every task, Success is a first-hitting event within the T T T -step evaluation window: the trajectory need not remain in the target set afterward or belong to it at the terminal step T T T . Task-specific feasibility conditions are checked through the first hitting step. Arrival time t s u c t_{\mathrm{suc}} t suc , in seconds, is summarized over successful trials only; minimum clearance c min c_{\min} c m i n is measured over the full evaluation window. The task tables use MS-MPPI.
Navigation
Problem Setup. We use the point-mass instance of (1) and the geometry specified in § 4.2 , with initial state x i n i t : = x 0 x_{\mathrm{init}} := x_0 x init := x 0 , initial position p i n i t : = p 0 p_{\mathrm{init}} := p_0 p init := p 0 , and target position p g p_g p g . The finite trajectory cost has the form
J x 0 ( U ) = ∑ k = 0 H − 1 ( q g o a l ∥ p k + 1 − p g ∥ 2 + q v e l ∥ v k + 1 ∥ 2 ) + ∑ k = 0 H − 1 ( q u ∥ u k ∥ 2 + q Δ u ∥ Δ u k ∥ 2 ) + q g o a l , f ∥ p H − p g ∥ 2 + q v e l , f ∥ v H ∥ 2 , (34) J_{x_0}(U) = \sum_{k=0}^{H-1} \left(q_{\mathrm{goal}}\lVert p_{k+1} - p_g\rVert^2 + q_{\mathrm{vel}}\lVert v_{k+1}\rVert^2\right) + \sum_{k=0}^{H-1} \left(q_u\lVert u_k\rVert^2 + q_{\Delta u}\lVert\Delta u_k\rVert^2\right) + q_{\mathrm{goal},f}\lVert p_H - p_g\rVert^2 + q_{\mathrm{vel},f}\lVert v_H\rVert^2, \tag*{(34)} J x 0 ( U ) = k = 0 ∑ H − 1 ( q goal ∥ p k + 1 − p g ∥ 2 + q vel ∥ v k + 1 ∥ 2 ) + k = 0 ∑ H − 1 ( q u ∥ u k ∥ 2 + q Δ u ∥ Δ u k ∥ 2 ) + q goal , f ∥ p H − p g ∥ 2 + q vel , f ∥ v H ∥ 2 , (34)
where Δ u 0 : = u 0 − u p r e v \Delta u_0 := u_0 - u_{\mathrm{prev}} Δ u 0 := u 0 − u prev and Δ u k : = u k − u k − 1 \Delta u_k := u_k - u_{k-1} Δ u k := u k − u k − 1 for k ≥ 1 k \ge1 k ≥ 1 , with u p r e v u_{\mathrm{prev}} u prev denoting the control applied at the preceding control step and set to zero at the beginning of an episode. The cost is minimized over U = ( u 0 , … , u H − 1 ) U = (u_0,\ldots,u_{H-1}) U = ( u 0 , … , u H − 1 ) subject to a componentwise input bound ∣ u k , j ∣ ≤ u max \lvert u_{k,j}\rvert\le u_{\max} ∣ u k , j ∣ ≤ u m a x . The cost (34) uses weights
q g o a l = 0.1 , q v e l = 0.05 , q u = 0.01 , q Δ u = 0.02 , q g o a l , f = 30 , q v e l , f = 10 , q_{\mathrm{goal}} = 0.1,\quad q_{\mathrm{vel}} = 0.05,\quad q_u = 0.01,\quad q_{\Delta u} = 0.02,\quad q_{\mathrm{goal},f} = 30,\quad q_{\mathrm{vel},f} = 10, q goal = 0.1 , q vel = 0.05 , q u = 0.01 , q Δ u = 0.02 , q goal , f = 30 , q vel , f = 10 ,
with input bound u max = 4 u_{\max} = 4 u m a x = 4 and prediction horizon H = 60 H = 60 H = 60 . Closed-loop evaluation uses T = 80 T = 80 T = 80 steps, corresponding to 8.0 s. For a realized trajectory, we define the minimum obstacle clearance over the evaluation window by
c min = min 0 ≤ k ≤ T , o ( ∥ p k − c o ∥ − r o ) , c_{\min} = \min_{0 \le k \le T,o}\left(\lVert p_k - c_o\rVert- r_o\right), c m i n = 0 ≤ k ≤ T , o min ( ∥ p k − c o ∥ − r o ) ,
where o o o indexes the obstacles and c o c_o c o and r o r_o r o denote the center and radius of obstacle o o o , respectively. Thus c min = 0 c_{\min} = 0 c m i n = 0 is feasible contact and c min < 0 c_{\min} < 0 c m i n < 0 is collision.
Under L t o t = 131,072 L_{\mathrm{tot}} = 131{,}072 L tot = 131 , 072 , MPPI uses L = 131,072 L = 131{,}072 L = 131 , 072 , MS-MPPI and CBO-MPPI use ( N , L , K ) = ( 256 , 64 , 8 ) (N,L,K) = (256,64,8) ( N , L , K ) = ( 256 , 64 , 8 ) , and CBO-MPC uses ( N , K ) = ( 16384 , 8 ) (N,K) = (16384,8) ( N , K ) = ( 16384 , 8 ) . These allocations satisfy the common budget in § 4 .
For CBO-MPPI, we use α = 0.02 \alpha= 0.02 α = 0.02 , Δ s = 0.2 \Delta s = 0.2 Δ s = 0.2 and σ c b o = 0.0913 \sigma_{\mathrm{cbo}} = 0.0913 σ cbo = 0.0913 , with initial-center standard deviation 2.0. At each control period, CBO-MPPI and CBO-MPC warm-start their shifted centers and particles, respectively, by adding independent smooth jitter with standard deviation 0.35. The local MPPI perturbation for CBO-MPPI has standard deviation 0.35; within each CBO evaluation round, a shared perturbation is used across centers.
Open-Loop Free-Energy Landscape. The routes in Figure 1 (left) are generated from nominal sequences { M h } h ∈ [ − 3 , 3 ] \{M_h\}_{h\in[-3,3]} { M h } h ∈ [ − 3 , 3 ] as follows. Let ϕ k = k / ( H − 1 ) \phi_k = k/(H-1) ϕ k = k / ( H − 1 ) denote normalized time and define s ( ϕ ) = ϕ 2 ( 1 − ϕ ) 2 s(\phi) = \phi^2(1-\phi)^2 s ( ϕ ) = ϕ 2 ( 1 − ϕ ) 2 and d ( ϕ ) = ϕ 2 ( 1 − ϕ ) 2 ( 1 − 2 ϕ ) d(\phi) = \phi^2(1-\phi)^2(1-2\phi) d ( ϕ ) = ϕ 2 ( 1 − ϕ ) 2 ( 1 − 2 ϕ ) . We write the goal displacement as Δ p = p g − p i n i t \Delta p = p_g-p_{\mathrm{init}} Δ p = p g − p init and the vertical unit vector as e y = ( 0 , 1 ) ⊤ e_y = (0,1)^\top e y = ( 0 , 1 ) ⊤ . With normalizing constants A s A_s A s and A h A_h A h , we set the velocity profile to
v k ( h ) = s ( ϕ k ) A s Δ p + h A h d ( ϕ k ) e y , k = 0 , … , H − 1 , v H ( h ) = 0. (35) v_k(h) = \frac{s(\phi_k)}{A_s}\Delta p + \frac{h}{A_h}d(\phi_k)e_y,\qquad k=0,\ldots,H-1,\qquad v_H(h)=0. \tag*{(35)} v k ( h ) = A s s ( ϕ k ) Δ p + A h h d ( ϕ k ) e y , k = 0 , … , H − 1 , v H ( h ) = 0. (35)
The bell function s s s is positive between the endpoints and vanishes at both endpoints. The antisymmetric function d d d is positive on ( 0 , 1 2 ) (0, \frac{1}{2}) ( 0 , 2 1 ) and negative on ( 1 2 , 1 ) (\frac{1}{2}, 1) ( 2 1 , 1 ) . We choose A s A_s A s so that the position integral of the first term equals Δ p \Delta p Δ p , and A h A_h A h so that the peak vertical deviation contributed by the second term equals h h h . The nominal control sequence M h = ( u 0 ( h ) , … , u H − 1 ( h ) ) M_h = (u_0(h), \ldots, u_{H-1}(h)) M h = ( u 0 ( h ) , … , u H − 1 ( h )) is obtained by finite-difference acceleration u k ( h ) = Π C u ( ( v k + 1 ( h ) − v k ( h ) ) / Δ t ) u_k(h) = \Pi_{C_u}\left((v_{k+1}(h)-v_k(h))/\Delta t\right) u k ( h ) = Π C u ( ( v k + 1 ( h ) − v k ( h )) /Δ t ) .
We use the following evaluation procedure for all landscapes below. Let r r r denote the landscape horizon. We first project each parameterized nominal sequence onto C u r C_u^r C u r . We then add perturbations to that projected nominal and project the resulting sequences again before rollout.
The nominal hard-constrained cost is J ˉ ( h ) : = J ˉ x i n i t ( M h ) \bar{J}(h) := \bar{J}_{x_{\mathrm{init}}}(M_h) J ˉ ( h ) := J ˉ x init ( M h ) . Using L M C L_{\mathrm{MC}} L MC control perturbations, the free-energy landscape estimator around M h M_h M h is
F ^ ( h ; τ ) = − τ log ( 1 L M C ∑ ℓ = 1 L M C e − J ˉ x i n i t ( Π C u H ( M h + ϵ ℓ ) ) / τ ) , (36) \widehat{F}(h;\tau) = -\tau\log\left(\frac{1}{L_{\mathrm{MC}}}\sum_{\ell=1}^{L_{\mathrm{MC}}} e^{-\bar{J}_{x_{\mathrm{init}}}\left(\Pi_{C_u^H}(M_h+\epsilon_\ell)\right)/\tau}\right), \tag*{(36)} F ( h ; τ ) = − τ log ( L MC 1 ℓ = 1 ∑ L MC e − J ˉ x init ( Π C u H ( M h + ϵ ℓ ) ) / τ ) , (36)
where L M C L_{\mathrm{MC}} L MC is the number of rollouts used only for this offline Monte Carlo landscape estimate, distinct from the controller batch size L L L and the post-hoc evaluation count L e L_e L e . The perturbations ϵ ℓ \epsilon_\ell ϵ ℓ follow (32) with marginal standard deviation 0.35 and temporal correlation ρ = 0.9 \rho= 0.9 ρ = 0.9 . Thus F ^ ( h ; τ ) \widehat{F}(h;\tau) F ( h ; τ ) is a Monte Carlo estimate of the projected free energy F x Π ( M h ) F_x^{\Pi}(M_h) F x Π ( M h ) . Each curve in Figure 1 (right) uses L M C = 65,536 L_{\mathrm{MC}} = 65{,}536 L MC = 65 , 536 antithetic Monte Carlo rollouts.
In a finer sweep with L M C = 262,144 L_{\mathrm{MC}} = 262{,}144 L MC = 262 , 144 antithetic rollouts, the transition temperature at the reference seed is approximately 21, and the gap F ^ ( 0 ) − min h F ^ ( h ) \widehat{F}(0) - \min_h \widehat{F}(h) F ( 0 ) − min h F ( h ) grows from about 6.0 to 55.9 and 215 at τ = 30 \tau= 30 τ = 30 , 100, and 300.
Closed-Loop Evaluation Details. A navigation trial succeeds at the first step k ≤ T k \le T k ≤ T such that ∥ p k − p g ∥ ≤ 0.6 \lVert p_k - p_g\rVert\le0.6 ∥ p k − p g ∥ ≤ 0.6 and the entire realized trajectory through step k k k is collision-free. Route classes use the crossing coordinate y c r o s s y_{\mathrm{cross}} y cross : Center if ∣ y c r o s s ∣ ≤ 0.15 \lvert y_{\mathrm{cross}}\rvert\le0.15 ∣ y cross ∣ ≤ 0.15 and Detour otherwise. Table 2 adds Success, route counts, arrival times, clearance, and implementation timing to the cost comparison in Table 1 . The closed-loop trajectories of MPPI, CBO-MPC, and CBO-MPPI are shown in Figure 2 .
Table 2. Closed-loop performance for Navigation at T = 80 T = 80 T = 80 . Metrics are medians over 100 trials; arrival times use successful trials only. A dash in t s u c t_{\mathrm{suc}} t suc denotes no successful trial.
MS-MPPI succeeds in 99/100 trials at τ = 15 \tau= 15 τ = 15 and 71/100 at τ = 300 \tau= 300 τ = 300 ; the other three methods succeed in all trials. MS-MPPI’s successful low-temperature trials arrive earlier than those of MPPI and CBO-MPPI. Arrival time is summarized over successful trials. At high temperature, CBO-MPPI’s detours arrive later than MPPI’s center routes.
Figure 3. Five-panel comparison for Many Obstacles over the common 12-s window. The outer panels show MPPI and CBO-MPPI at τ = 15 \tau= 15 τ = 15 (upper) and τ = 3000 \tau= 3000 τ = 3000 (lower); the center shows CBO-MPC. Each panel overlays all 100 trials.
Many Obstacles
Problem Setup. Many Obstacles uses the same point-mass instance of (1) and hard-obstacle convention as Navigation, with fourteen circular obstacles over [ − 10 , 10 ] 2 [-10, 10]^2 [ − 10 , 10 ] 2 , initial state ( − 8.5 , − 9 , 0 , 0 ) (-8.5, -9, 0, 0) ( − 8.5 , − 9 , 0 , 0 ) , and goal ( 8.5 , 9 ) (8.5, 9) ( 8.5 , 9 ) . The weights are ( q g o a l , q v e l , q Δ u , q g o a l , f , q v e l , f ) = ( 0.015 , 0.02 , 0.005 , 0.03 , 80 , 20 ) (q_{\mathrm{goal}}, q_{\mathrm{vel}}, q_{\Delta u}, q_{\mathrm{goal},f}, q_{\mathrm{vel},f}) = (0.015, 0.02, 0.005, 0.03, 80, 20) ( q goal , q vel , q Δ u , q goal , f , q vel , f ) = ( 0.015 , 0.02 , 0.005 , 0.03 , 80 , 20 ) , u max = 2.5 u_{\max} = 2.5 u m a x = 2.5 , and H = 50 H = 50 H = 50 . Closed-loop evaluation uses T = 120 T = 120 T = 120 steps, corresponding to 12.0 s. Under L t o t = 131,072 L_{\mathrm{tot}} = 131{,}072 L tot = 131 , 072 , MPPI uses L = 131,072 L = 131{,}072 L = 131 , 072 , MS-MPPI and CBO-MPPI use ( N , L , K ) = ( 256 , 64 , 8 ) (N, L, K) = (256, 64, 8) ( N , L , K ) = ( 256 , 64 , 8 ) , and CBO-MPC uses ( N , K ) = ( 16384 , 8 ) (N, K) = (16384, 8) ( N , K ) = ( 16384 , 8 ) . CBO-MPPI uses α = 0.02 \alpha= 0.02 α = 0.02 , σ c b o = 0.1 \sigma_{\mathrm{cbo}} = 0.1 σ cbo = 0.1 , and Δ s = 0.2 \Delta s = 0.2 Δ s = 0.2 , with initial-center standard deviation 2.5 and warm-start jitter standard deviation 0.50; the local perturbation has standard deviation 0.45.
Closed-Loop Performance. A trial succeeds at the first step k ≤ T = 120 k \le T = 120 k ≤ T = 120 such that ∥ p k − p g ∥ ≤ 0.6 \lVert p_k - p_g \rVert\le0.6 ∥ p k − p g ∥ ≤ 0.6 and the entire realized trajectory through step k k k is collision-free. To evaluate F C L F_{\mathrm{CL}} F CL , we use perturbation standard deviation 0.45.
At τ = 15 \tau= 15 τ = 15 , MPPI, CBO-MPC, and CBO-MPPI succeed in all 100 trials; MS-MPPI succeeds in 81. CBO-MPC has the lowest free energy, and MS-MPPI has the highest realized cost and free energy. At τ = 3000 \tau= 3000 τ = 3000 , CBO-MPPI succeeds in 90 trials, MS-MPPI in 40, and MPPI and CBO-MPC in all 100. Both MS-MPPI and CBO-MPPI lower free energy relative to MPPI and have higher realized cost. CBO-MPPI improves both costs and the success rate relative to MS-MPPI. Its larger-clearance trajectories reach the goal earlier among successful trials than MPPI and CBO-MPC.
Table 3. Closed-loop performance for Many Obstacles at T = 120 T = 120 T = 120 . Metrics are medians over 100 trials; arrival times use successful trials only. A dash in t s u c t_{\mathrm{suc}} t suc denotes no successful trial.
Pendulum
We next conduct torque-limited pendulum swing-up experiments to test whether the J J J –F x I I F_x^{\mathrm{II}} F x II gap extends beyond hard-obstacle navigation.
Problem Setup. We impose the bound ∣ u ∣ ≤ u max |u| \le u_{\max} ∣ u ∣ ≤ u m a x on the actuator torque. The state is x = ( θ , ω ) x = (\theta,\omega) x = ( θ , ω ) with θ = 0 \theta= 0 θ = 0 the upright target and θ = π \theta= \pi θ = π the downward initial position. The continuous-time dynamics are
θ ˙ = ω , ω ˙ = g l sin θ − c m p l 2 ω + u m p l 2 , \dot{\theta} = \omega,\qquad\dot{\omega} = \frac{g}{l}\sin\theta- \frac{c}{m_p l^2}\omega+ \frac{u}{m_p l^2}, θ ˙ = ω , ω ˙ = l g sin θ − m p l 2 c ω + m p l 2 u ,
discretized by the fourth-order Runge–Kutta method (RK4) with Δ t = 0.05 \Delta t = 0.05 Δ t = 0.05 , g = 9.81 g = 9.81 g = 9.81 , pendulum length l = 1 l = 1 l = 1 , mass m p = 1 m_p = 1 m p = 1 , damping c = 0.05 c = 0.05 c = 0.05 , and input bound u max = 3 u_{\max} = 3 u m a x = 3 ; the discrete model saturates the angular velocity at ∣ ω ∣ ≤ 8 |\omega| \le8 ∣ ω ∣ ≤ 8 . At the specified parameters, u max / ( m p g l ) ≈ 0.31 u_{\max}/(m_pgl) \approx0.31 u m a x / ( m p g l ) ≈ 0.31 . The available torque is insufficient to reach the upright in a single half-swing. Swing-up therefore requires energy pumping over multiple oscillations.
Define the wrapped angle error by
e θ ( θ ) : = atan2 ( sin θ , cos θ ) . e_\theta(\theta) := \operatorname{atan2}(\sin\theta,\cos\theta). e θ ( θ ) := atan2 ( sin θ , cos θ ) .
The regulation cost uses
ℓ ( x t , u t ) = e θ ( θ t ) 2 + 0.1 ω t 2 , g ( x H ) = e θ ( θ H ) 2 + 0.1 ω H 2 , \ell(x_t,u_t) = e_\theta(\theta_t)^2 + 0.1\omega_t^2,\qquad g(x_H) = e_\theta(\theta_H)^2 + 0.1\omega_H^2, ℓ ( x t , u t ) = e θ ( θ t ) 2 + 0.1 ω t 2 , g ( x H ) = e θ ( θ H ) 2 + 0.1 ω H 2 ,
with no control-penalty term.
A torque disturbance ν t ∼ N ( 0 , 0.5 2 ) \nu_t \sim\mathcal{N}(0,0.5^2) ν t ∼ N ( 0 , 0. 5 2 ) is added to each applied input. The equal-budget comparison uses L t o t = 16,384 L_{\mathrm{tot}} = 16{,}384 L tot = 16 , 384 with planning horizon H = 80 H = 80 H = 80 . Closed-loop evaluation uses T = 170 T = 170 T = 170 steps, corresponding to 8.5 s. MPPI uses L = 16,384 L = 16{,}384 L = 16 , 384 , MS-MPPI and CBO-MPPI use ( N , L , K ) = ( 256 , 16 , 4 ) (N,L,K) = (256,16,4) ( N , L , K ) = ( 256 , 16 , 4 ) , and CBO-MPC uses ( N , K ) = ( 4096 , 4 ) (N,K) = (4096,4) ( N , K ) = ( 4096 , 4 ) , so each method evaluates 16,384 rollouts per step. The MPPI-based controllers use perturbation standard deviation 0.25. CBO-MPPI uses α = 1 \alpha= 1 α = 1 , σ c b o = 0.1118 \sigma_{\mathrm{cbo}} = 0.1118 σ cbo = 0.1118 , Δ s = 0.1 \Delta s = 0.1 Δ s = 0.1 , initial-center standard deviation 1.0 with temporal correlation 0.9, and warm-start jitter with standard deviation 0.3. The closed-loop visualization and quantitative comparison use τ ∈ { 1 , 10 } \tau\in\{1,10\} τ ∈ { 1 , 10 } for the MPPI-based controllers, together with the single temperature-independent CBO-MPC configuration.
Open-Loop Free-Energy Landscape. We compare the swing-up strategies favored by J J J and F ^ Π \hat{F}^{\Pi} F ^ Π in the obstacle-free Pendulum task. We use the landscape-evaluation procedure of Section C.2 with the following family of torque profiles. Two parameters ( a , b ) (a,b) ( a , b ) index the sinusoidal open-loop torque profiles
u t ( a , b ) = a sin ( 2 π ⋅ 3 t H ′ ) + b sin ( 2 π ⋅ 4 t H ′ ) , t = 0 , … , H ′ − 1. u_t(a,b) = a \sin\left(\frac{2\pi\cdot3t}{H'}\right) + b \sin\left(\frac{2\pi\cdot4t}{H'}\right), \qquad t = 0,\ldots,H'-1. u t ( a , b ) = a sin ( H ′ 2 π ⋅ 3 t ) + b sin ( H ′ 2 π ⋅ 4 t ) , t = 0 , … , H ′ − 1.
For this landscape calculation only, we use the longer horizon H ′ = 160 H' = 160 H ′ = 160 (i.e., 8 s) with the same stage cost ℓ \ell ℓ as the closed loop. For each of the 81 × 81 81 \times81 81 × 81 grid points in ( a , b ) (a,b) ( a , b ) -space, F ^ ( a , b ; τ ) \hat{F}(a,b;\tau) F ^ ( a , b ; τ ) is estimated as in (36) from L M C = 4096 L_{\mathrm{MC}} = 4096 L MC = 4096 perturbations following (32) with marginal standard deviation 0.25 and temporal correlation ρ = 0.9 \rho= 0.9 ρ = 0.9 . Since this example has no state constraints, J ˉ x \bar{J}_x J ˉ x equals J x J_x J x on every input-projected sequence.
Free energy favors a swing-up strategy with a wider perturbation basin in this task. Figure 4 shows the open-loop cost landscape J ( a , b ) J(a,b) J ( a , b ) and the free-energy landscape F ^ ( a , b ; 10 ) \hat{F}(a,b;10) F ^ ( a , b ; 10 ) over this sinusoidal template family, with markers identifying the minimizer of each objective. To compare the trajectories’ energy levels, let E u p = 2 m p g ℓ E_{\mathrm{up}} = 2m_{\mathrm{p}}g\ell E up = 2 m p g ℓ denote the mechanical energy at the upright equilibrium, measured relative to the downward equilibrium. The minimizer of J J J is a cost-efficient “tightrope” trajectory: it pumps the mechanical energy to 107% of E u p E_{\mathrm{up}} E up , crosses θ = 0 \theta= 0 θ = 0 with nonzero angular velocity, and lies in a narrow valley of the cost landscape. The minimizer of F ^ \hat{F} F ^ uses a “soft-landing” strategy, pumping to almost exactly 100% of E u p E_{\mathrm{up}} E up with near-zero angular velocity at the upright. This strategy has slightly higher nominal J J J and lies in a wider low-cost neighborhood.
Figure 4. Open-loop landscape for Pendulum over sinusoidal templates (H ′ = 160 H' = 160 H ′ = 160 , τ = 10 \tau= 10 τ = 10 ). Left: nominal cost J ( a , b ) J(a,b) J ( a , b ) . Right: free energy F ^ ( a , b ; 10 ) \hat{F}(a,b;10) F ^ ( a , b ; 10 ) . Red ×: arg min J \operatorname{arg\,min} J arg min J (energy overshoot 7%). Orange +: arg min F ^ \operatorname{arg\,min} \hat{F} arg min F ^ (overshoot ≤ 0.2 % \le0.2\% ≤ 0.2% ).
Closed-Loop Performance. We next compare swing-up attainment and closed-loop performance to assess the effect of the search mechanism under the same rollout budget. A trial succeeds at the first state satisfying
∣ e θ ( θ k ) ∣ < 0.25 , ∣ ω k ∣ < 1.0 , k ≤ T . \lvert e_{\theta}(\theta_k)\rvert< 0.25,\qquad\lvert\omega_k\rvert< 1.0,\qquad k \le T. ∣ e θ ( θ k )∣ < 0.25 , ∣ ω k ∣ < 1.0 , k ≤ T .
The realized cost J C L J_{\mathrm{CL}} J CL is accumulated over the full evaluation window using the disturbed trajectory. To evaluate F C L F_{\mathrm{CL}} F CL , we use perturbation standard deviation 0.25. Figure 5 shows the phase portraits, and Table 4 summarizes the quantitative results.
Figure 5. Five-panel phase-portrait comparison for Pendulum over the common 8.5-s window. The outer panels show MPPI and CBO-MPPI at τ = 1 \tau= 1 τ = 1 (upper) and τ = 10 \tau= 10 τ = 10 (lower); the center shows the temperature-independent CBO-MPC baseline. Each panel overlays all 100 trials.
Table 4. Closed-loop performance for Pendulum at T = 170 T = 170 T = 170 . Metrics are medians over 100 trials; arrival times use successful trials only. A dash in t s u c t_{\mathrm{suc}} t suc denotes no successful trial.
MPPI, CBO-MPC, and CBO-MPPI succeed in all trials at both temperatures. MS-MPPI succeeds in 96/100 trials at τ = 1 \tau= 1 τ = 1 and 98/100 at τ = 10 \tau= 10 τ = 10 . At τ = 1 \tau= 1 τ = 1 , MS-MPPI reaches the target later and has higher realized cost and free energy than MPPI and CBO-MPPI. CBO-MPPI has lower median free energy than MPPI together with slightly later arrival and higher realized cost. Appendix C.7 shows the opposite low-temperature paired-median ordering throughout the parameter sweeps. At τ = 10 \tau= 10 τ = 10 , MS-MPPI improves both costs relative to MPPI, and CBO-MPPI improves them further and arrives earlier than MPPI. CBO-MPC has the lowest realized cost; CBO-MPPI has the lowest free energy.
Planar Arm
Finally, we conduct experiments with a redundant planar manipulator to investigate whether the free-energy objective favors larger-clearance postures by exploiting kinematic redundancy under hard link-obstacle constraints.
Problem Setup. The arm has three revolute joints with state x = ( q , v ) ∈ R 6 x=(q,v)\in\mathbb{R}^{6} x = ( q , v ) ∈ R 6 , where q ∈ R 3 q\in\mathbb{R}^{3} q ∈ R 3 is the joint angle and v ∈ R 3 v\in\mathbb{R}^{3} v ∈ R 3 is the joint velocity. We use a joint-space damped double-integrator model, where the link lengths and trigonometric dependence enter through forward kinematics and clearance evaluation.
With time step Δ t = 0.1 \Delta t=0.1 Δ t = 0.1 , damping d = 1 d=1 d = 1 , and joint-space acceleration input u ∈ R 3 u\in\mathbb{R}^{3} u ∈ R 3 , the discrete dynamics are
q k + 1 = q k + Δ t v k , v k + 1 = ( 1 − d Δ t ) v k + Δ t u k , q_{k+1}=q_k+\Delta t\,v_k,\qquad v_{k+1}=(1-d\Delta t)v_k+\Delta t\,u_k, q k + 1 = q k + Δ t v k , v k + 1 = ( 1 − d Δ t ) v k + Δ t u k ,
with componentwise input bound ∣ u k , j ∣ ≤ 5 |u_{k,j}|\leq5 ∣ u k , j ∣ ≤ 5 . For link lengths ℓ 1 = ℓ 2 = ℓ 3 = 1 \ell_1=\ell_2=\ell_3=1 ℓ 1 = ℓ 2 = ℓ 3 = 1 , the joint positions are
p i ( q ) = ∑ j = 1 i ℓ j [ cos ( ∑ m = 1 j q m ) sin ( ∑ m = 1 j q m ) ] , i = 1 , 2 , 3 , (37) p_i(q)=\sum_{j=1}^{i}\ell_j
\begin{bmatrix}
\cos\left(\sum_{m=1}^{j}q_m\right)\\
\sin\left(\sum_{m=1}^{j}q_m\right)
\end{bmatrix},
\qquad i=1,2,3, \tag*{(37)} p i ( q ) = j = 1 ∑ i ℓ j cos ( ∑ m = 1 j q m ) sin ( ∑ m = 1 j q m ) , i = 1 , 2 , 3 , (37)
with base point p 0 = ( 0 , 0 ) p_0=(0,0) p 0 = ( 0 , 0 ) and end-effector position p e e ( q ) = p 3 ( q ) p_{\mathrm{ee}}(q)=p_3(q) p ee ( q ) = p 3 ( q ) . The objective includes end-effector tracking, velocity damping, input effort, and input smoothness. For an input sequence U = ( u 0 , … , u H − 1 ) U=(u_0,\ldots,u_{H-1}) U = ( u 0 , … , u H − 1 ) , define the finite tracking and control cost
J x 0 ( U ) = ∑ k = 0 H − 1 ( q e e ∥ p e e ( q k + 1 ) − p g ∥ 2 + q v ∥ v k + 1 ∥ 2 ) + ∑ k = 0 H − 1 ( q u ∥ u k ∥ 2 + q Δ u ∥ Δ u k ∥ 2 ) + Q e e ∥ p e e ( q H ) − p g ∥ 2 + Q v ∥ v H ∥ 2 , (38) J_{x_0}(U)=\sum_{k=0}^{H-1}\left(q_{\mathrm{ee}}\lVert p_{\mathrm{ee}}(q_{k+1})-p_g\rVert^2+q_v\lVert v_{k+1}\rVert^2\right)+\sum_{k=0}^{H-1}\left(q_u\lVert u_k\rVert^2+q_{\Delta u}\lVert\Delta u_k\rVert^2\right)+Q_{\mathrm{ee}}\lVert p_{\mathrm{ee}}(q_H)-p_g\rVert^2+Q_v\lVert v_H\rVert^2, \tag*{(38)} J x 0 ( U ) = k = 0 ∑ H − 1 ( q ee ∥ p ee ( q k + 1 ) − p g ∥ 2 + q v ∥ v k + 1 ∥ 2 ) + k = 0 ∑ H − 1 ( q u ∥ u k ∥ 2 + q Δ u ∥ Δ u k ∥ 2 ) + Q ee ∥ p ee ( q H ) − p g ∥ 2 + Q v ∥ v H ∥ 2 , (38)
where Δ u 0 : = u 0 − u p r e v \Delta u_0:=u_0-u_{\mathrm{prev}} Δ u 0 := u 0 − u prev and Δ u k : = u k − u k − 1 \Delta u_k:=u_k-u_{k-1} Δ u k := u k − u k − 1 for k ≥ 1 k\geq1 k ≥ 1 , with u p r e v u_{\mathrm{prev}} u prev denoting the control applied at the preceding control step and set to zero at the beginning of an episode. The numerical weights are q e e = 10 q_{\mathrm{ee}}=10 q ee = 10 , q v = 0.2 q_v=0.2 q v = 0.2 , q u = 2 × 10 − 3 q_u=2\times10^{-3} q u = 2 × 1 0 − 3 , q Δ u = 10 − 2 q_{\Delta u}=10^{-2} q Δ u = 1 0 − 2 , Q e e = 200 Q_{\mathrm{ee}}=200 Q ee = 200 , and Q v = 1 Q_v=1 Q v = 1 . The prediction horizon is H = 20 H=20 H = 20 , and closed-loop evaluation uses T = 35 T=35 T = 35 steps, corresponding to 3.5 3.5 3.5 s. The initial configuration is q 0 = ( − 0.8 , 1.0 , − 0.6 ) q_0=(-0.8,1.0,-0.6) q 0 = ( − 0.8 , 1.0 , − 0.6 ) , and the target end-effector position is ( 1.8 , 1.2 ) (1.8,1.2) ( 1.8 , 1.2 ) . Two circular obstacles are placed at ( 1.0 , 0.6 ) (1.0,0.6) ( 1.0 , 0.6 ) and ( 1.5 , 0.9 ) (1.5,0.9) ( 1.5 , 0.9 ) with radii 0.25 0.25 0.25 and 0.20 0.20 0.20 . We measure the link-obstacle clearance at configuration q q q by
c a r m ( q ) = min i , o ( d s e g ( [ p i − 1 ( q ) , p i ( q ) ] , c o ) − r o − r ℓ ) , (39) c_{\mathrm{arm}}(q)=\min_{i,o}\left(d_{\mathrm{seg}}([p_{i-1}(q),p_i(q)],c_o)-r_o-r_\ell\right), \tag*{(39)} c arm ( q ) = i , o min ( d seg ([ p i − 1 ( q ) , p i ( q )] , c o ) − r o − r ℓ ) , (39)
where i i i and o o o index the links and obstacles, respectively, c o c_o c o and r o r_o r o are the obstacle center and radius, r ℓ = 0.05 r_\ell=0.05 r ℓ = 0.05 is the link radius, and d s e g d_{\mathrm{seg}} d seg is the point-to-segment distance. For a realized trajectory, we define the minimum link-obstacle clearance over the evaluation window by
c min : = min 0 ≤ k ≤ T c a r m ( q k ) . c_{\min}:=\min_{0\leq k\leq T}c_{\mathrm{arm}}(q_k). c m i n := 0 ≤ k ≤ T min c arm ( q k ) .
Zero clearance is feasible contact; negative clearance indicates link-obstacle penetration. After input projection, we enforce obstacle avoidance by assigning cost + ∞ +\infty + ∞ to rollouts with min 0 ≤ k ≤ H c a r m ( q k ) < 0 \min_{0\leq k\leq H}c_{\mathrm{arm}}(q_k)<0 min 0 ≤ k ≤ H c arm ( q k ) < 0 . The same J ˉ x \bar{J}_x J ˉ x is used in the landscape calculation, the closed-loop controllers, and all closed-loop metrics involving hard link-obstacle feasibility. For the closed-loop comparison, the MPPI-based controllers are evaluated at τ ∈ { 3 , 300 } \tau\in\{3,300\} τ ∈ { 3 , 300 } and use perturbation standard deviation 0.3. CBO-MPPI uses α = 1 \alpha= 1 α = 1 , σ c b o = 0.1291 \sigma_{\mathrm{cbo}} = 0.1291 σ cbo = 0.1291 , Δ s = 0.1 \Delta s = 0.1 Δ s = 0.1 , initial-center standard deviation 1.0 with temporal correlation 0.81, warm-start jitter standard deviation 0.2, and ( N , L , K ) = ( 256 , 64 , 8 ) (N,L,K) = (256,64,8) ( N , L , K ) = ( 256 , 64 , 8 ) . MPPI uses L = 131,072 L = 131{,}072 L = 131 , 072 rollouts per MPC step. CBO-MPC uses ( N , K ) = ( 16384 , 8 ) (N,K) = (16384,8) ( N , K ) = ( 16384 , 8 ) , α = 1 \alpha= 1 α = 1 , and σ c b o = 0.1291 \sigma_{\mathrm{cbo}} = 0.1291 σ cbo = 0.1291 ; its 16384 × 8 16384 \times8 16384 × 8 evaluations give the same L t o t = 131,072 L_{\mathrm{tot}} = 131{,}072 L tot = 131 , 072 as MPPI and CBO-MPPI.
Open-Loop Free-Energy Landscape. We evaluate J ˉ x \bar{J}_x J ˉ x and F x Π F_x^\Pi F x Π along the arm’s redundant degree of freedom using the landscape-evaluation procedure of Section C.2. The three-link arm is kinematically redundant for a two-dimensional end-effector target. For this landscape calculation only, we use the 10-Hz discretization Δ t = 0.1 \Delta t = 0.1 Δ t = 0.1 s and landscape horizon H ′ = 60 H' = 60 H ′ = 60 . We construct a one-parameter family of nominal control sequences M h M_h M h from a fixed closed-loop input sequence U b a s e = ( u 0 b a s e , … , u H ′ − 1 b a s e ) U^{\mathrm{base}} = (u^{\mathrm{base}}_0,\ldots,u^{\mathrm{base}}_{H'-1}) U base = ( u 0 base , … , u H ′ − 1 base ) . Let q ⋆ q_\star q ⋆ be the final configuration reached by this base sequence, and let n ( q ⋆ ) n(q_\star) n ( q ⋆ ) be a unit vector in the null space of the end-effector Jacobian at q ⋆ q_\star q ⋆ . For normalized time ϕ k = k / ( H ′ − 1 ) \phi_k = k/(H'-1) ϕ k = k / ( H ′ − 1 ) , define the smooth ramp r ( ϕ ) = ϕ 2 ( 3 − 2 ϕ ) r(\phi) = \phi^2(3-2\phi) r ( ϕ ) = ϕ 2 ( 3 − 2 ϕ ) and set M h = ( u 0 ( h ) , … , u H ′ − 1 ( h ) ) M_h = (u_0(h),\ldots,u_{H'-1}(h)) M h = ( u 0 ( h ) , … , u H ′ − 1 ( h )) , where
u k ( h ) = Π C u ( u k b a s e + 2 h r ( ϕ k ) n ( q ⋆ ) ) , k = 0 , … , H ′ − 1 , h ∈ [ − 0.3 , 0.3 ] . (40) u_k(h) = \Pi_{\mathcal{C}_u}\left(u_k^{\mathrm{base}} + 2h\,r(\phi_k)n(q_\star)\right), \qquad k = 0,\ldots,H'-1,\quad h \in[-0.3,0.3]. \tag*{(40)} u k ( h ) = Π C u ( u k base + 2 h r ( ϕ k ) n ( q ⋆ ) ) , k = 0 , … , H ′ − 1 , h ∈ [ − 0.3 , 0.3 ] . (40)
The scalar h h h therefore moves the terminal posture in the local null-space direction: the end-effector remains near the target as the elbow and wrist posture change, altering link-obstacle clearance. For each h h h , the nominal hard-constrained objective is J ˉ ( h ) = J ˉ x 0 ( M h ) \bar{J}(h) = \bar{J}_{x_0}(M_h) J ˉ ( h ) = J ˉ x 0 ( M h ) . Using L M C = 1024 L_{\mathrm{MC}} = 1024 L MC = 1024 Gaussian perturbations with standard deviation 0.03 and temporal correlation ρ = 0.81 \rho= 0.81 ρ = 0.81 , we estimate F x Π F_x^\Pi F x Π by F ^ ( h ; τ ) \hat{F}(h;\tau) F ^ ( h ; τ ) as in (36) . Figure 6 shows representative final postures induced by several values of h h h and the resulting J ˉ x \bar{J}_x J ˉ x and F ^ \hat{F} F ^ landscapes.
Figure 6. Posture parameterization and landscape for Planar Arm at 10 Hz (H ′ = 60 H' = 60 H ′ = 60 ). Varying h h h changes the redundant posture and keeps the end effector near the goal. Dotted circles in (a) have radius r o + r ℓ r_o + r_\ell r o + r ℓ and mark the contact boundaries for the link centerlines, accounting for the link radius r ℓ = 0.05 r_\ell= 0.05 r ℓ = 0.05 . Curves in (b) are normalized to [ 0 , 1 ] [0, 1] [ 0 , 1 ] .
The preferred redundant posture moves toward larger-clearance neighborhoods under the free-energy objective, and the shift grows with temperature (Figure 6 b). In the reference sweep, J ˉ x \bar{J}_x J ˉ x is minimized at h = − 0.10 h=-0.10 h = − 0.10 with c a r m ( q H ′ ) = 0.0276 c_{\mathrm{arm}}(q_{H'})=0.0276 c arm ( q H ′ ) = 0.0276 . The free-energy minimizers move to h = − 0.04 h=-0.04 h = − 0.04 and h = + 0.09 h=+0.09 h = + 0.09 for τ = 3 \tau=3 τ = 3 and τ = 300 \tau=300 τ = 300 , with c a r m ( q H ′ ) = 0.0278 c_{\mathrm{arm}}(q_{H'})=0.0278 c arm ( q H ′ ) = 0.0278 and 0.0284, respectively. The posture sweep uses perturbation standard deviation 0.03; the closed-loop MPC comparison uses horizon H = 20 H=20 H = 20 and standard deviation 0.3.
Closed-Loop Performance. We next examine how objective choice and search mechanism affect target attainment and obstacle clearance in closed-loop operation. A trial succeeds at the first step k ≤ T k \le T k ≤ T satisfying
∥ p e e ( q k ) − p g ∥ < 0.20 , ∥ v k ∥ < 1.5 , \lVert p_{\mathrm{ee}}(q_k)-p_g\rVert< 0.20,\qquad\lVert v_k\rVert< 1.5, ∥ p ee ( q k ) − p g ∥ < 0.20 , ∥ v k ∥ < 1.5 ,
provided that the entire realized trajectory through step k k k is feasible, i.e., c a r m ( q j ) ≥ 0 c_{\mathrm{arm}}(q_j) \ge0 c arm ( q j ) ≥ 0 for every 0 ≤ j ≤ k 0 \le j \le k 0 ≤ j ≤ k . The realized cost J C L J_{\mathrm{CL}} J CL uses (38) over the full evaluation window, and we use perturbation standard deviation 0.3 to evaluate F C L F_{\mathrm{CL}} F CL . Figure 7 shows the final postures, and Table 5 summarizes the quantitative results.
Table 5. Closed-loop performance for Planar Arm at T = 35 T = 35 T = 35 . Metrics are medians over 100 trials; arrival times use successful trials only. A dash in t s u c t_{\mathrm{suc}} t suc denotes no successful trial.
Figure 7. Five-panel comparison of final postures for Planar Arm at the end of the common 3.5-s window. The outer panels show MPPI and CBO-MPPI at τ = 3 \tau= 3 τ = 3 (upper) and τ = 300 \tau= 300 τ = 300 (lower); the center shows the temperature-independent CBO-MPC baseline. Each panel overlays all 100 trials.
At τ = 3 \tau=3 τ = 3 , all four controllers succeed in all trials. MS-MPPI has the lowest realized cost and free energy and the earliest median arrival. CBO-MPC is second on both costs, while CBO-MPPI improves both over MPPI; independent local search remains best here. At τ = 300 \tau=300 τ = 300 , MPPI never meets the target criterion, MS-MPPI succeeds in 71/100 trials, CBO-MPPI succeeds in 91/100, and CBO-MPC succeeds in all trials. CBO-MPC’s 100/100 count means that every trajectory enters the target set while feasible, with median first arrival at 1.10 s; it does not require residence in the target set at the terminal time. Its direct optimization of trajectory cost aligns with the target-error and velocity conditions in the Success criterion, whereas high-temperature CBO-MPPI optimizes the perturbation-averaged objective and attains larger clearance with later arrival. CBO-MPPI has lower free energy and realized cost than MS-MPPI; MS-MPPI’s larger median clearance accompanies a lower success rate. Relative to CBO-MPC, CBO-MPPI attains larger clearance (0.132 versus 0.003), lower free energy, and higher realized cost.
Effect of the Prediction Horizon
The reference settings fix one prediction horizon per task. Changing the horizon changes both the available lookahead and the dimension of the decision variable.
Protocol. For each task, we rerun the closed-loop experiment over a grid of horizons at that task’s own L t o t L_{\mathrm{tot}} L tot and temperature, with twenty seeds per cell and the seed family of the task tables. The control period and the T T T -step evaluation window stay fixed as H H H varies, so the panels compare controllers on one fixed control problem. We compare MPPI, MS-MPPI, and CBO-MPPI. MS-MPPI and CBO-MPPI share the allocation ( N , L , K ) (N, L, K) ( N , L , K ) , local perturbations, temperature, center selection, and warm start. MS-MPPI applies an MPPI update to each center; CBO-MPPI updates all centers through consensus drift and diffusion. CBO-MPPI keeps the noise-to-drift ratio κ = D σ c b o 2 / ( 2 λ ) \kappa= D\sigma_{\mathrm{cbo}}^{2}/(2\lambda) κ = D σ cbo 2 / ( 2 λ ) of its reference setting, that is σ c b o ( H ) = 2 λ κ / ( m H ) \sigma_{\mathrm{cbo}}(H) = \sqrt{2\lambda\kappa/(mH)} σ cbo ( H ) = 2 λκ / ( m H ) with m m m the input dimension, so the isotropic condition 2 λ > D σ c b o 2 2\lambda> D\sigma_{\mathrm{cbo}}^{2} 2 λ > D σ cbo 2 holds at every horizon shown; MPPI keeps its reference perturbation scale. MPPI and CBO-MPPI optimize free energy, so we score each episode by both its realized cost J C L J_{\mathrm{CL}} J CL and the free energy F C L F_{\mathrm{CL}} F CL of the realized input sequence, as defined in (13) . We compute F C L F_{\mathrm{CL}} F CL post hoc at the higher of the two temperatures used for that task. The evaluation perturbation has fixed dimension m T mT m T , whereas a per-solve free energy has dimension m H mH m H .
The preferred horizon depends on the task, controller, and criterion. The curves in Figure 8 are generally nonmonotone, and their minima span an order of magnitude across tasks. Free energy is lowest at H = 30 H = 30 H = 30 on Navigation for all three controllers; on Pendulum, the minima occur at H = 160 H = 160 H = 160 , 40 40 40 , and 60 60 60 for MPPI, MS-MPPI, and CBO-MPPI; and on Planar Arm, they occur at H = 60 H = 60 H = 60 , 15 15 15 , and 15 15 15 . The corresponding realized-cost minima occur at H = ( 30 , 10 , 20 ) H = (30, 10, 20) H = ( 30 , 10 , 20 ) on Navigation, H = ( 160 , 20 , 80 ) H = (160, 20, 80) H = ( 160 , 20 , 80 ) on Pendulum, and H = ( 10 , 5 , 5 ) H = (10, 5, 5) H = ( 10 , 5 , 5 ) on Planar Arm, in the same controller order. Thus realized cost favors the shortest tested horizon for MS-MPPI and CBO-MPPI on Planar Arm, with medians 235.4 and 192.1. Longer horizons can also carry a substantial penalty: on Pendulum, moving from H = 80 H = 80 H = 80 to H = 320 H = 320 H = 320 raises median realized cost by 44%, 59%, and 73% for MPPI, MS-MPPI, and CBO-MPPI, and leaves them with 6/20, 1/20, and 16/20 successes.
Figure 8. Realized cost (top) and free energy of the realized input sequence (bottom) against the prediction horizon, with the horizontal axis showing the lookahead H Δ t H\Delta t H Δ t . Each task uses its own L t o t L_{\mathrm{tot}} L tot (131,072 for Navigation and Planar Arm and 16,384 for Pendulum) and reference temperature (τ = 300 \tau= 300 τ = 300 for Navigation and Planar Arm and τ = 10 \tau= 10 τ = 10 for Pendulum). All three controllers retain their other reference settings, CBO-MPPI keeping its reference κ \kappa κ ; MS-MPPI differs from CBO-MPPI in the center update alone, so the vertical gap between the orange and green curves is the effect of the consensus term. Curves are medians over twenty seeds and bands are the interquartile range. The Planar Arm panel starts at the shortest tested horizon, H = 5 H = 5 H = 5 . The dotted line marks the horizon used elsewhere in the paper.
The consensus term often sustains the long-horizon regime. MS-MPPI and CBO-MPPI draw the same number of rollouts from the same number of centers, allowing a direct comparison of their center updates. At short horizons, independent centers suffice and sometimes perform better: at H = 10 H = 10 H = 10 on Navigation, MS-MPPI attains the lower realized cost (96.2 against 137.8) and free energy (797.1 against 916.6), and at H = 10 H = 10 H = 10 on Pendulum, it succeeds in 15/20 trials where CBO-MPPI succeeds in none, with both the lower cost (919.9 against 1163.8) and the lower free energy (841.3 against 1109.2). As the horizon grows, CBO-MPPI retains more successes on Navigation and Pendulum: MS-MPPI falls to 15/20 at H = 60 H = 60 H = 60 and 11/20 at H = 120 H = 120 H = 120 on Navigation, and to 7/20 at H = 160 H = 160 H = 160 and 1/20 at H = 320 H = 320 H = 320 on Pendulum, whereas CBO-MPPI records 19/20, 17/20, 20/20, and 16/20. On Planar Arm, CBO-MPPI leads 14/20 to 5/20 at H = 30 H = 30 H = 30 , while the two methods have similar success at H = 40 H = 40 H = 40 and equal success at H = 60 H = 60 H = 60 . At the three reference horizons, CBO-MPPI has lower realized cost and higher success than MS-MPPI: 151.7 against 340.1 and 19/20 against 15/20 on Navigation; 561.6 against 697.8 and 20/20 against 19/20 on Pendulum; and 285.8 against 332.4 and 16/20 against 15/20 on Planar Arm. Its free energy is also lower on Navigation (823.4 against 909.5) and Planar Arm (750.1 against 785.4), while MS-MPPI is slightly lower on Pendulum (740.0 against 742.7). At H = 60 H = 60 H = 60 on Planar Arm, MS-MPPI similarly has lower free energy (891.0 against 904.1), CBO-MPPI has slightly lower realized cost (431.1 against 438.9), and both succeed in 7/20 trials. MPPI trails MS-MPPI and CBO-MPPI over most of the grid: it meets the Planar Arm success criterion at no horizon, succeeds in 6/20 trials at H = 320 H = 320 H = 320 on Pendulum, and falls to 6/20 at H = 120 H = 120 H = 120 on Navigation.
Parameter Sensitivity
Using the reference controller settings in Appendices C.§ 1 –C.§ 5 , we vary one parameter at a time over the grids
α / α r e f ∈ { 0.25 , 0.5 , 1 , 2 , 4 , 8 } , κ = D σ c b o 2 2 λ ∈ { 0.1 , 0.3 , 0.5 , 0.7 , 0.9 } . \alpha/\alpha_{\mathrm{ref}} \in\{0.25, 0.5, 1, 2, 4, 8\}, \qquad\kappa= \frac{D\sigma_{\mathrm{cbo}}^{2}}{2\lambda} \in\{0.1, 0.3, 0.5, 0.7, 0.9\}. α / α ref ∈ { 0.25 , 0.5 , 1 , 2 , 4 , 8 } , κ = 2 λ D σ cbo 2 ∈ { 0.1 , 0.3 , 0.5 , 0.7 , 0.9 } .
Each parameter setting uses the same 20 independent trials paired seed-wise with MPPI. For trial r r r within a fixed task–temperature case, let F C B O ( r ) ( θ ) F_{\mathrm{CBO}}^{(r)}(\theta) F CBO ( r ) ( θ ) denote the CBO-MPPI F C L F_{\mathrm{CL}} F CL at a swept parameter value θ \theta θ , and let F M P P I ( r ) F_{\mathrm{MPPI}}^{(r)} F MPPI ( r ) denote the paired MPPI F C L F_{\mathrm{CL}} F CL . We measure sensitivity to the reference CBO-MPPI setting and the difference from MPPI using the following medians of paired differences over trials r r r :
Δ F r e f ( θ ) : = median r [ F C B O ( r ) ( θ ) − F C B O ( r ) ( θ r e f ) ] . \Delta F_{\mathrm{ref}}(\theta) := \operatorname{median}_{r}\left[F_{\mathrm{CBO}}^{(r)}(\theta) - F_{\mathrm{CBO}}^{(r)}(\theta_{\mathrm{ref}})\right]. Δ F ref ( θ ) := median r [ F CBO ( r ) ( θ ) − F CBO ( r ) ( θ ref ) ] .
Δ F M P P I ( θ ) : = median r [ F C B O ( r ) ( θ ) − F M P P I ( r ) ] . \Delta F_{\mathrm{MPPI}}(\theta) := \operatorname{median}_{r}\left[F_{\mathrm{CBO}}^{(r)}(\theta) - F_{\mathrm{MPPI}}^{(r)}\right]. Δ F MPPI ( θ ) := median r [ F CBO ( r ) ( θ ) − F MPPI ( r ) ] .
Here θ r e f \theta_{\mathrm{ref}} θ ref is α / α r e f = 1 \alpha/\alpha_{\mathrm{ref}} = 1 α / α ref = 1 or κ = 0.5 \kappa= 0.5 κ = 0.5 . Negative values indicate lower free energy than the reference CBO-MPPI setting and paired MPPI, respectively. Because the median is taken after forming each seed-wise difference, these quantities may differ from differences of the corresponding direct medians. Table 6 –Table 7 report all settings and lower-F F F counts.
Table 6. Complete α / α r e f \alpha/\alpha_{\mathrm{ref}} α / α ref sensitivity. Direct F F F columns are medians; difference columns are medians of seed-wise paired differences and may differ from differences of the displayed direct medians. Lower-F F F counts trials with F C B O - M P P I < F M P P I F_{\mathrm{CBO\text{-}MPPI}} < F_{\mathrm{MPPI}} F CBO - MPPI < F MPPI . Each parameter setting summarizes 20 paired trials.
Table 7. Complete κ \kappa κ sensitivity. Direct F F F columns are medians; difference columns are medians of seed-wise paired differences and may differ from differences of the displayed direct medians. Lower-F F F counts trials with F C B O - M P P I < F M P P I F_{\mathrm{CBO\text{-}MPPI}} < F_{\mathrm{MPPI}} F CBO - MPPI < F MPPI .
Across the 72 non-reference settings, Δ F r e f \Delta F_{\mathrm{ref}} Δ F ref is negative at 34 and positive at 38, and every one of the 16 task–temperature–parameter sweeps contains at least one non-reference setting with negative Δ F r e f \Delta F_{\mathrm{ref}} Δ F ref . Thus non-reference settings both increase and decrease the achieved free energy relative to the reference setting. After identifying the reference setting shared by the two parameter sweeps in each task–temperature case, the experiment contains 80 unique settings. Relative to MPPI, Δ F M P P I < 0 \Delta F_{\mathrm{MPPI}} < 0 Δ F MPPI < 0 at 62 of these 80 settings: all 40 high-temperature settings and 22 of 40 low-temperature settings. Pendulum at τ = 1 \tau= 1 τ = 1 is the exception, with Δ F M P P I > 0 \Delta F_{\mathrm{MPPI}} > 0 Δ F MPPI > 0 throughout both sweeps.
In the tables, F M P P I F_{\mathrm{MPPI}} F MPPI and F C B O - M P P I F_{\mathrm{CBO\text{-}MPPI}} F CBO - MPPI denote median F C L F_{\mathrm{CL}} F CL . The lower-F F F counts complement the paired medians by showing how often CBO-MPPI has smaller F C L F_{\mathrm{CL}} F CL than paired MPPI.
Some parameter settings combine lower free energy with lower success rates. For example, Many Obstacles at τ = 15 \tau= 15 τ = 15 , κ = 0.9 \kappa= 0.9 κ = 0.9 succeeds in 7/20 trials, and Navigation at τ = 300 \tau= 300 τ = 300 , κ = 0.9 \kappa= 0.9 κ = 0.9 succeeds in 5/20. The Many Obstacles κ = 0.9 \kappa= 0.9 κ = 0.9 cells retain three numerical failures at τ = 15 \tau= 15 τ = 15 and four at τ = 3000 \tau= 3000 τ = 3000 in the 20-seed denominators. Both cases lie in the tested range κ < 1 \kappa< 1 κ < 1 , equivalently 2 λ − D σ c b o 2 > 0 2\lambda- D\sigma_{\mathrm{cbo}}^{2} > 0 2 λ − D σ cbo 2 > 0 , used by the idealized mean-field theory.