Fast and Robust Temporal Logic Planning via ADMM-based Trajectory Optimization
Authors: Lukas Pries, Joris Verhagen, Jon Arrizabalaga, Jana Tumova, Markus Ryll, Zachary Manchester
Organizations: Department of Aerospace and Geodesy, TU Munich, Germany · Division of Robotics, Perception, and Learning, KTH Royal Institute of Technology, Stockholm, Sweden · Department of Aeronautics and Astronautics, MIT, Cambridge, MA, USA
We present a fast numerical method for safe continuous-time motion planning under Temporal Logic (TL) specifications. The method generates smooth continuous trajectories that remain collision-free while robustly satisfying temporal and logical task requirements. A central component of our method is the formulation of nonconvex safety and logic constraints as unions of convex sets where associated discrete decisions are encoded in a joint feasibility graph. This graph representation allows Euclidean projection onto the feasible set and proximal robustness maximization to be reformulated as shortest- and widest-path problems, respectively. Building on this structure, we develop a nonconvex splitting method based on the Alternating Direction Method of Multipliers (ADMM), which decouples smooth spatio-temporal trajectory optimization from nonsmooth discrete constraint handling within the optimization. The resulting algorithm exhibits reliable convergence across benchmarks and scales to large-scale motion-planning problems, providing a 4.7x average speedup over the state of the art on discrete and continuous-time logic problems.
Figures & tables
Fig. 1: Our ADMM solver on an underground cave-rescue scenario. A temporal logic specification requires visiting the rescue Randys and phones as specified in Section VII . The total product graph of the Transition- and Logic Graph has 28 million vertices, beyond the capacity of existing methods. The final trajectory is shown in red and visited boxes shaded blue.
Fig. 2: Temporal operator automata. Timed automata for the Eventually , Always , Eventually Always and Until operators. The array [t1,t2] denotes the interval in which the global time t must reside. Spatial robustification is achieved by robustifying the predicate satisfaction via piρ:μ(⋅)≥ρϕ and temporal robustification is achieved by extending the intervals. F[t1,t2]G[t3,t4]μ is robustified by recording the reset time, tr←t , in the center of [t1,t2] and observing μ “ θϕ ” longer and shorter than required by [t3,t4] . Similarly, ϕ1U[t1,t2]ϕ2 is robustified by observing μ1 earlier than necessary, and switching to μ2 in the middle of the interval I=[t1,t2] .
Fig. 3: Graph representations. (a) The free space is covered by overlapping convex regions So and the trajectory is split into segments m=1,…,M . (b) The possible assignments of segments to adjacent regions form the transition graph GS . (c) The STL specification ϕ is compiled into a timed automaton and (d) unrolled over the same segments into the logic graph Gϕ . (e) Their product G=GS⊗Gϕ is the feasibility graph: each layer holds one vertex per region–logic-state pair, so a path (red) from the initial vertex to an accepting one is a segment-to-region assignment that jointly satisfies S and ϕ .
Fig. 4: ADMM Iterations: (a) The primal update solves an unconstrained spatio-temporal minimization, yielding a continuous trajectory x (blue). The slack update requires projecting onto the nonconvex feasible set, ΠF(x) , and robustness maximization, proxδϕF(x) . Both are resolved by first projecting each segment xm onto each set Fm,k (b) and solving a Pareto-optimal graph-search problem with associated costs (c). The dual gradient ascent step gradually enforces consensus between primal and slack variables. It acts as a cost update that shifts the primal solution towards feasibility as depicted in (d).
ADMM 5 th order
GCS+STL 5 th order
STL MIQP 5 th order
STL MILP 2 nd order
Scenario
t [s]
θ
ρ
t [s]
θ
ρ
t [s]
θ
ρ
t [s]
θ
ρ
stlcg
0.74
3.11
0.20
11.78
0.00
0.00
22.30
4.10
0.20
0.34
4.95
0.20
puzzle-1
2.88
12.96
0.50
0.48
0.00
0.00
–
–
–
–
–
–
rover
5.99
13.21
0.70
349.34
0.01
0.00
–
–
–
100.08
19.30
0.70
either-or
1.42
7.78
0.55
65.53
0.29
0.13
–
–
–
–
–
–
deliver
7.54
2.51
0.50
23.92
1.00
0.09
–
–
–
29.29
3.78
0.50
TABLE I: Comparison of our ADMM solver against GCS+STL [ 10 ] and MIQP/MILP [ 20 ] in several scenarios. A ‘ − ’ denotes a timeout after no solution is found at 10−3 tolerance within 1200 seconds. Our ADMM method obtains maximal spatial robustness and close to optimal temporal robustness at a reduced computational time compared to the MIQP/MILP. We outperform the GCS+STL benchmark [ 10 ] on all but one scenario while simultaneously being able to maximize the robustness metrics.
Figure 6
Fig. 5: (a) Runtime for the GCS formulation and our ADMM approach as a function of the maze size. Results are averaged over five runs, each maze is n×n . (b) A single 10×10 maze.
Fig. 6: (a) Distinct trajectories for different robustness types; shortest path by visiting N , spatially robust by visiting S , and temporally robust by visiting T . (b) The degree of satisfaction of the chosen predicate μi(t) over time; spatially robust maximizing the value, temporally robust maximizing the interval.
School of Automation & Intelligent Sensing, Shanghai Jiao Tong University, Shanghai 200240, China · Institute of Artificial Intelligence, Xiamen University, Xiamen, China