arXiv
Abstract
Safety-aware motion planning remains a challenge in robotics, especially when missions are time-critical and are under complex specifications. In this paper, we propose safety-aware-stl-mppi, a computationally efficient sampling-based receding-horizon planning framework designed to promote satisfaction of constraints expressed in Signal Temporal Logic (STL). Our approach encodes discrete-time STL formulas into candidate time-varying control barrier functions (CBF), which are integrated into a model predictive path integral (MPPI) controller. Our method inherits the benefits of low computational cost from an efficiently parallelizable sampling-based planner and utilizes CBF for constraints expressed in STL. We compare against several MPPI baselines using four artificial Mars Rover planning case studies with a diverse environment and cost setups, where we show our method consistently achieving high safety and efficiency. We show a quadcopter planning experiment with NVIDIA Isaac Lab.
The Problem
Time-critical missions come with timed, spatio-temporal safety rules. Sampling-based planners such as MPPI are fast but offer no safety guarantee, while optimization-based STL planners are too slow to run online.
Key Idea
Encode the STL formula as a time-varying control barrier function, use it to weight MPPI's rollouts, and project the weighted rollout onto the CBF's safe set with a cheap closed-form step.
Result
100% empirical STL satisfaction across all four Mars Rover case studies, with and without actuation limits, with significantly lower computation time than robustness- and penalty-based MPPI.
Problem Formulation
We consider motion planning problems in which a robot must satisfy a safety requirement expressed as a signal temporal logic formula while minimizing a user-defined cost. The specification and the cost are treated as separate objects: the cost encodes mission preferences, such as conserving actuation effort or maintaining smooth motion, and may be unrelated to or even in conflict with the safety requirement. The planner must optimize the cost while promoting satisfaction of the specification.
System and specifications
We consider discrete-time control-affine systems \(x_{t+1} = f(x_t, u_t) := f_0(x_t) + g(x_t)\,u_t\) with state \(x_t \in \mathbb{R}^n\) and input \(u_t \in \mathcal{U} \subseteq \mathbb{R}^m\) under box actuation limits. Safety requirements are expressed in a fragment of signal temporal logic (STL):
$$\begin{aligned} \varphi &:= \top \;\mid\; \pi^{\mu} \;\mid\; \lnot\pi^{\mu} \;\mid\; \varphi_1 \land \varphi_2,\\[2pt] \phi &:= G_{[a,b]}\varphi \;\mid\; F_{[a,b]}\varphi \;\mid\; \varphi_1\, U_{[a,b]}\, \varphi_2 \;\mid\; \phi_1 \land \phi_2 \;\mid\; \varphi \end{aligned}$$The atomic elements are predicates \(\pi^{\mu}\), defined through predicate functions \(\mu : \mathbb{R}^n \to \mathbb{R}\) with \(\pi^\mu(x_t) = \top\) iff \(\mu(x_t) \ge 0\) — e.g. signed distances to obstacles or goal regions. \(G_{[a,b]}\) (always), \(F_{[a,b]}\) (eventually) and \(U_{[a,b]}\) (until) impose timed requirements over the bounded horizon \(H\). STL also carries robust semantics \(\rho^{\phi}(x, \tau_0) \in \mathbb{R}\), measuring how robustly a trajectory satisfies \(\phi\).
Receding-horizon planning
At each time \(t\), given a planning horizon \(\mathcal{H} \le H - t\), stage cost \(q\) and terminal cost \(E\), the planner solves the open-loop optimal control problem
$$v_t^{*,(t)},\dots,v_{t+\mathcal{H}-1}^{*,(t)} := \operatorname*{arg\,min}_{u_t,\dots,u_{t+\mathcal{H}-1}\in\,\mathcal{U}} \Big\{ E\big(x_{t+\mathcal{H}}^{(t)}\big) + \textstyle\sum_{\tau=t}^{t+\mathcal{H}-1} q\big(x_\tau^{(t)}, u_\tau\big) \;\;\text{s.t.}\;\; x_{\tau+1}^{(t)} = f\big(x_\tau^{(t)}, u_\tau\big) \Big\},$$executes the first input, and re-plans — producing the closed-loop trajectory \(x^*\).
Problem 1. Given the system \(f\), the initial state \(x_0\), a planning horizon \(\mathcal{H}\), and cost functions \(q\) and \(E\), design a feedback control such that \((x^*, 0) \models \phi\), where \(\phi\) is the user-defined STL specification. At each time \(t \in \{0,\dots,H-1\}\), we would like to optimize the OCP above.
The difficulty is that the OCP knows nothing about \(\phi\): a planner that simply minimizes cost may be driven straight through unsafe regions, while encoding \(\phi\) into the cost (via robust semantics) requires expensive long-horizon rollouts and offers no satisfaction mechanism.
Method
A safety instruction becomes an STL formula, the formula becomes a time-varying CBF, and the CBF steers MPPI twice: first by re-weighting sampled rollouts, then by projecting the weighted rollout back into the safe set.
1 · Encoding STL as a time-varying CBF
Following the continuous-time construction of Lindemann and Dimarogonas, we adapt the encoding of STL formulas into time-varying control barrier functions to discrete time. A differentiable function \(h : D \times \mathbb{Z}_{\ge 0} \to \mathbb{R}\) induces the time-varying set \(\mathcal{C}_t := \{x \in D \mid h(x,t) \ge 0\}\).
Definition 1 (Time-varying CBF). \(h\) is a candidate CBF for \(f\) if \(\mathcal{C}_t \neq \emptyset\) for all \(t \in \{0,\dots,H\}\). A candidate CBF is valid if for every \(t \in \{0,\dots,H-1\}\) and every \(x \in \mathcal{C}_t\) there exists \(u \in \mathcal{U}\) such that $$h\big(f(x,u),\, t+1\big) - h(x,t) \;\ge\; -\alpha\, h(x,t), \qquad \alpha \in (0,1].$$
Theorem 1 (Forward invariance). Let \(x_0 \in \mathcal{C}_0\) and let every input satisfy the constraint above. Then \(x_t \in \mathcal{C}_t\) for all \(t \in \{0,\dots,H\}\).
Given \(\phi\), a CBF \(h^{\phi}\) is constructed recursively. A predicate \(\pi^\mu\) contributes a barrier term — \(\mu\) itself, or a smooth surrogate. Each temporal operator contributes a term that depends explicitly on time: for an eventually requirement \(F_{[a,b]}\,\varphi\), for example, the term tightens as the deadline \(b\) approaches, so the set it permits gradually contracts toward the states where \(\varphi\) holds, forcing the system to reach them in time. Conjunctions are combined through a smooth under-approximation of the minimum,
$$\min_{i\in\{1,\dots,p\}} h_i(x,t) \;\ge\; \widetilde{\min}\big(h_1(x,t),\dots,h_p(x,t)\big) := -\ln\Big(\textstyle\sum_{i=1}^{p} \exp\big(-h_i(x,t)\big)\Big),$$following the encoding rules below. Rows 2–4 encode temporal operators over predicates, rows 5–7 over conjunctions, and the last row conjunctions over temporal operators. A deletion mechanism removes each term once its time window has passed, reducing conservatism.
Theorem 2 (Safety from the STL CBF). Let \(x_0 \in \mathcal{C}_0\) and suppose a valid CBF is constructed following the encoding rules. Then enforcing the CBF constraint at every step implies \((x,0) \models \phi\).
2 · CBF-informed weighted rollouts
MPPI samples \(K\) control perturbations \(\omega^{k,(t)}\) with \(\omega_\tau^{k,(t)} \sim \mathcal{N}(0, \Sigma_\omega)\) around a nominal sequence \(v^{(t)}\) and rolls them out in parallel. The standard sample cost
$$J^{k,(t)} := E\big(x^{k,(t)}_{t+\mathcal{H}}\big) + \sum_{\tau=t}^{t+\mathcal{H}-1} \Big[ q\big(x^{k,(t)}_\tau, u^{k,(t)}_\tau\big) + \gamma\, \big(v^{(t)}_\tau\big)^{T} \Sigma_\omega^{-1} \omega_\tau^{k,(t)} \Big]$$carries no information about \(\phi\), so we augment it with the CBF-constraint violation along each rollout:
$$J_\phi^{k,(t)} := J^{k,(t)} + \eta \sum_{\tau=t}^{t+\mathcal{H}-1} C_{\mathrm{CBF}}\big(x^{k,(t)}, \tau\big), \qquad C_{\mathrm{CBF}} := \max\Big\{0,\; (1-\alpha)\, h^{\phi}\big(x^{k,(t)}_{\tau},\tau\big) - h^{\phi}\big(x^{k,(t)}_{\tau+1},\tau+1\big)\Big\}.$$Rollouts that drift out of the shrinking safe set receive lower weights in the weighted average \(v^{+,(t)} = \sum_k W^{k,(t)} u^{k,(t)} / \sum_k W^{k,(t)}\) with \(W^{k,(t)} := \exp[-\tfrac{1}{\lambda}(J_\phi^{k,(t)} - \min_i J_\phi^{i,(t)})]\). Compared with placing STL robustness in the cost, \(C_{\mathrm{CBF}}\) is evaluated per step — no shrinking-horizon robustness computation is needed.
3 · A lightweight CBF projection
The weighted rollout still carries no guarantee, so each of its first \(N{+}1\) inputs is projected onto the CBF constraint. Because the exact constraint is nonconvex in discrete time, a first-order Taylor expansion of \(h^{\phi}\) around \(x^{(t)}_\tau\) with \(A(x^{(t)}_\tau, \tau) := \tfrac{\partial h^{\phi}}{\partial x}\big(x^{(t)}_\tau, \tau+1\big)\) yields the affine condition
$$h^{\phi}\big(x^{(t)}_\tau, \tau+1\big) + A\big(x^{(t)}_\tau,\tau\big)^{T} \Big[ f_0\big(x^{(t)}_\tau\big) - x^{(t)}_\tau + g\big(x^{(t)}_\tau\big)\, v^{s}_\tau \Big] \;\ge\; (1-\alpha)\, h^{\phi}\big(x^{(t)}_\tau, \tau\big),$$and the projection becomes a tiny quadratic program, where \(\Pi\) and \(b\) stack the CBF half-space with the active actuation limits:
$$\hat{v}^{s,(t)}_\tau = \operatorname*{arg\,min}_{v^s_\tau} \tfrac{1}{2}\big(v^{s}_\tau - v^{+,(t)}_\tau\big)^{T}\big(v^{s}_\tau - v^{+,(t)}_\tau\big) \quad \text{s.t.} \quad \Pi\, v^{s}_\tau \le b,$$which is solved efficiently at runtime and, when \(\mathcal{U} = \mathbb{R}^m\), in closed form:
$$\hat{v}^{s,(t)}_\tau = v^{+,(t)}_\tau - a_{\mathrm{cbf}}\big(x^{(t)}_\tau,\tau\big)\, \frac{\max\big\{0,\; a_{\mathrm{cbf}}^{T} v^{+}_\tau - b_{\mathrm{cbf}}\big\}} {\big\|a_{\mathrm{cbf}}\big(x^{(t)}_\tau,\tau\big)\big\|_2^2}.$$
Because the real-time implementation enforces the first-order approximation rather than the exact constraint, perfect safety is not guaranteed theoretically; the paper discusses an optional robustification (Theorem 3 in the appendix) and evaluates satisfaction empirically — reaching 100% across all case studies.
Results
Four artificial Mars Rover missions with different dynamics, goals and STL specifications, each run with and without actuation limits, plus a photorealistic quadcopter task in NVIDIA Isaac Lab.
Mars Rover case studies
Case A uses nonlinear dynamics, Cases B and C a single integrator with timed reach-avoid goals (in Case C the goal location may be unreachable under the specification), and Case D a double integrator among 13 obstacles. We compare against vanilla MPPI, reach-avoid MPPI, robustness-based MPPI and penalty-based MPPI. Success rate (SR) is the share of 20 closed-loop trials with non-negative STL robustness, and timing is the average closed-loop planning time in seconds. All methods satisfied the actuation limits in every trial.
Success rate in the two hardest cases. Vanilla and reach-avoid MPPI cannot cope with tight timed constraints, and the robustness-based and penalty-based variants only partly satisfy them.
Full comparison
| (a) Limited actuation | Case A | Case B | Case C | Case D | ||||
|---|---|---|---|---|---|---|---|---|
| SR ↑ | Timing ↓ | SR ↑ | Timing ↓ | SR ↑ | Timing ↓ | SR ↑ | Timing ↓ | |
| Safety-aware-STL-MPPI (Ours) | 100% | 5.26 | 100% | 8.80 | 100% | 8.84 | 100% | 17.93 |
| Vanilla MPPI | 100% | 1.35 | 100% | 3.32 | 0% | 3.33 | 0% | 5.97 |
| Reach-avoid MPPI | 100% | 5.92 | 100% | 7.31 | 0% | 7.32 | 25% | 17.05 |
| Robustness-based MPPI | 100% | 190.35 | 95% | 90.74 | 90% | 90.84 | 55% | 1060.55 |
| Penalty-based MPPI | 100% | 191.16 | 80% | 91.21 | 85% | 91.34 | 60% | 1057.51 |
| (b) Unlimited actuation | Case A | Case B | Case C | Case D | ||||
|---|---|---|---|---|---|---|---|---|
| SR ↑ | Timing ↓ | SR ↑ | Timing ↓ | SR ↑ | Timing ↓ | SR ↑ | Timing ↓ | |
| Safety-aware-STL-MPPI (Ours) | 100% | 3.80 | 100% | 7.40 | 100% | 7.37 | 100% | 16.39 |
| Vanilla MPPI | 100% | 1.55 | 100% | 3.31 | 0% | 3.31 | 0% | 5.90 |
| Reach-avoid MPPI | 100% | 3.47 | 100% | 5.19 | 0% | 5.21 | 25% | 14.55 |
| Robustness-based MPPI | 100% | 202.87 | 100% | 90.32 | 100% | 90.31 | 55% | 1057.29 |
| Penalty-based MPPI | 95% | 203.45 | 100% | 90.65 | 95% | 90.59 | 60% | 1057.07 |
Success rate (SR) and closed-loop planning time (seconds) over 20 trials per case. Our method achieves a 100% empirical STL satisfaction rate in every evaluated case. It is significantly faster than the robustness- and penalty-based variants.
Quadcopter simulation in Isaac Lab
A Crazyflie quadcopter must reach a goal region near (0, 0, 1) within 5 s in an artificial factory with seven obstacles (three package tables, a two-level stand and three forklifts) while staying above the ground. Planning uses a 3D double integrator at 200 Hz with K = 100 rollouts. Over 50 trials every plan respected the actuation limit, with an average planning time of 15.21 s. The planned waypoints were then tracked in Isaac Sim, and 100% of the tracked trajectories satisfied the specification.
BibTeX