SANDO: Safe Autonomous Trajectory Planning for Dynamic Unknown Environments

Kota Kondo; Jesús Tordesillas; Juan Rached; Lili Sun; Yixuan Jia; Jonathan P. How

cs.ROApr 8, 2026

SANDO: Safe Autonomous Trajectory Planning for Dynamic Unknown Environments

Authors: Kota Kondo, Jesús Tordesillas, Juan Rached, Lili Sun, Yixuan Jia, Jonathan P. How

Organizations: the Department of Aeronautics and Astronautics, Massachusetts Institute of Technology, Cambridge, MA 02139 USA · IIT, ICAI, Comillas Pontifical University, Madrid, Spain · the Trustworthy Robotics and AI Lab (TRAIL)

Abstract

This paper presents SANDO, a safe trajectory planner for 3D dynamic unknown environments. Existing soft-constraint planners are fast but do not guarantee collision-free paths, while hard-constraint methods typically ensure safety at the cost of longer computation. SANDO addresses this trade-off through three contributions. First, a heat map-based A* global planner steers the path away from high-risk regions, and a spatiotemporal safe flight corridor (STSFC) generator produces time-layered polytopes that inflate obstacles only by their worst-case reachable set at each time layer, rather than over the entire horizon. Second, trajectory optimization is formulated as a mixed-integer quadratic program with hard collision-avoidance constraints, and variable elimination reduces the number of decision variables. Third, a formal safety analysis establishes collision-free guarantees under explicit velocity-bound, size-bound, and estimation-error assumptions. Ablation studies confirm that variable elimination yields up to 7.4 times faster optimization and that STSFCs are critical for feasibility in dense dynamic environments. In simulations against state-of-the-art methods, SANDO achieves a 100% success rate across all forest and dynamic benchmark difficulty levels with no constraint violations, and perception-only experiments demonstrate the full perception-to-planning pipeline. Hardware experiments with fully onboard planning, perception, and localization demonstrate six safe flights in static environments and twelve among dynamic obstacles.

Figures & tables

Fig. 1: Comparison of trajectory planning approaches near a dynamic obstacle $\mathcal{O}^{d}$ . Left (Other Approaches – Soft): Soft-constraint methods use penalty-based collision avoidance, producing fast but potentially unsafe trajectories. Center (Other Approaches – Hard): Other hard-constraint methods inflate the obstacle by its full worst-case reachable set and generate purely spatial corridors, producing safe but overly conservative trajectories that detour far from the obstacle. Right (SANDO): SANDO generates spatiotemporal safe flight corridors (STSFCs) that account for when the trajectory passes each region, inflating the obstacle only by its reachable set at the corresponding time layer, yielding a safe and fast trajectory.

Panel of Table 2 — TABLE I: State-of-the-art UAV trajectory planners in unknown environments. Safety guarantee: ✓ = continuous, $\triangle$ = discretized, ✗ = none.

Fig. 2: SANDO system overview: Point cloud data are processed by the Map Manager, which detects static obstacles (denoted $\mathcal{O}^{s}$ ) and dynamic obstacles (denoted $\mathcal{O}^{d}$ ) and estimates the states of dynamic obstacles. The Dynamic Obstacle Tracker feeds its predictions to the Heat Map module, which generates soft costs around both static and dynamic obstacles (Section V ). The heat map-based global planner computes a global path using A ∗ search, and the predicted obstacle trajectories are used by the STSFC Generation module to produce time-layered polytope sequences (Section IV ). These time-varying polytopes are then used in the Trajectory Optimization module to compute a safe trajectory (Section VI ).

Fig. 3: Unknown-space inflation for STSFC corridor generation. Boundary voxels of unknown regions are inflated by the same per-layer $\epsilon$ -buffered reachable set $\mathcal{R}_{n}^{\epsilon}$ as dynamic obstacles (shown as an L2 ball in this figure; the per-axis box variant is analogous). Scenario 1: The goal $G_{1}$ lies outside the unknown region. The trajectory from $A_{1}$ to $G_{1}$ stays outside the inflated boundary, guaranteeing safety against unobserved dynamic obstacles. Scenario 2: The goal $G_{2}$ lies inside the unknown region. SANDO walks back along the global path from the unknown boundary until a point outside the worst-case inflated region is found, yielding a new subgoal $G_{2}^{\prime}$ .

Fig. 4: STSFC and MIQP trajectory optimization in dynamic environments. For visualization, the figure shows a 2D ( $x$ - $y$ ) case with the time-layer axis as the third dimension; in practice, SANDO uses full 3D ( $x$ , $y$ , $z$ ) STSFCs. Left column, other approaches (hard-blocking reachable sets): At $t=t_{0}$ (top left), the planner hard-blocks the worst-case reachable set of $\mathcal{O}^{d}$ as occupied and computes a conservative global path $A\to G$ that avoids the entire blocked region. Although planning succeeds, the resulting trajectory is overly conservative. At $t=t_{1}$ (bottom left), the agent replans from the new position $A^{\prime}$ toward $G^{\prime}$ , but $A^{\prime}$ now falls inside the shifted blocked region, making the query $A^{\prime}\to G^{\prime}$ infeasible. Right column, SANDO (heat map-based soft costs): At $t=t_{0}$ , the 2D view (top center) shows the global path $A\to G$ steered away from $\mathcal{O}^{d}$ by soft heat costs, producing a less conservative trajectory. The temporal view (top right) visualizes the STSFC corridors $\mathcal{C}[n][p]$ along the time-layer axis, with increasing per-layer inflation; the MIQP assigns each trajectory piece to a corridor ( $\bm{x}_{0}\subseteq\mathcal{C}[0][0]$ , $\bm{x}_{1}\subseteq\mathcal{C}[1][1]$ , $\bm{x}_{2}\subseteq\mathcal{C}[2][1]$ , and $\bm{x}_{3}\subseteq\mathcal{C}[3][2]$ ). At $t=t_{1}$ , the 2D view (bottom center) and temporal view (bottom right) show successful replanning from $A^{\prime}$ to $G^{\prime}$ : the updated corridors $\mathcal{C}^{\prime}[n][p]$ remain valid and the MIQP assignment ( $\bm{x}^{\prime}_{0}\subseteq\mathcal{C}^{\prime}[0][0]$ , $\bm{x}^{\prime}_{1}\subseteq\mathcal{C}^{\prime}[1][1]$ , $\bm{x}^{\prime}_{2}\subseteq\mathcal{C}^{\prime}[2][2]$ , and $\bm{x}^{\prime}_{3}\subseteq\mathcal{C}^{\prime}[3][2]$ ) succeeds, since soft costs never blocked the region around $A^{\prime}$ . Even though in the first global path section ( $A^{\prime}\to W_{1}$ ), the corridor generation for $n=2$ (green) and $n=3$ (blue) fails to produce corridors, since $n=0$ (pink) and $n=1$ (red) still have valid corridors, the MIQP can find a feasible trajectory that safely navigates through the dynamic environment.

Fig. 5: Visualization of the heat map-based global planner in hardware. From the wall on the side, the static heat decays with distance, while from the dynamic obstacle (blue and green boxes), the heat decays both spatially and temporally. On the left, the dynamic obstacle is not moving, so the heat is dominated by the base heat around its current position. On the right, the dynamic obstacle is moving toward the center of the room (the prediction is visualized as the green line), so the tube penalty creates a heat trail along the predicted path, steering the global path away from that region.

Module	Symbol	Value	Parameter
Static Heat Map	$\alpha^{s}$	5.0	Intensity scale
	$p^{s}$	2	Decay exponent
	$R^{s}$	$3.0\cdot r_{\text{drone}}$	Halo radius
	$H_{\max}$	50.0	Maximum heat
Dynamic Heat Map	$\alpha^{d}_{0}$	1.0	Base cost scale
	$\alpha^{d}_{1}$	2.0	Tube penalty scale

TABLE II: SANDO system parameters across all simulations.

Fig. 6: Standardized benchmarking environment: start at the left, goals at the right, safe flight corridors as green polytopes, trajectories color-coded by planner. (a) Trajectories for 10 of the cases; (b) close-up of one case, where SUPER’s soft constraints pull the trajectory toward polytope centers, yielding longer paths.

Algorithm		Thread	N	Success	Computation Time		Performance			Constraint Violation
Algorithm		Thread	N	$R^{\mathrm{opt}}_{\mathrm{succ}}$ [%]	$T^{\mathrm{per}}_{\mathrm{opt}}$ [ms]	$T^{\mathrm{total}}_{\mathrm{opt}}$ [ms]	$T_{\mathrm{trav}}$ [s]	$L_{\mathrm{path}}$ [m]	$S_{\mathrm{jerk}}$ [m/s 2 ]	$\rho_{\mathrm{SFC}}$ [%]	$\rho_{\mathrm{vel}}$ [%]	$\rho_{\mathrm{acc/jerk}}$ [%]
SUPER	( $L_{2}$ )	multi	–	100.0	0.6		14.1	6.8	1.3	0.2	0.5	0.0
SUPER	( $L_{\infty}$ )	multi	–	100.0	0.5		13.6	6.9	1.5	0.3	4.0	0.0
FASTER	(orig.)	single	4	93.4	7.9	48.1	13.0	6.8	1.4	0.0	5.0	0.0
FASTER	(CP)	single		93.4	4.5	36.5	13.2	6.8	1.4	0.0	0.0	0.0
SANDO		multi		93.4	1.3	3.2	13.2	6.8	1.4	0.0	0.0	0.0

TABLE IV: Local trajectory optimization benchmarking results against FASTER [ 2 ] and SUPER [ 4 ] (computation time, performance, and constraint violation). SANDO and FASTER (CP) with

N=6

achieve the shortest travel time among the configurations with no constraint violations, and SANDO achieves substantially faster computation time than FASTER (CP) due to multi-threading. We mark in green the best value in each column and in red the worst value.

Fig. 7: Static Benchmarking: The static forest Gazebo environment used for benchmarking SANDO against state-of-the-art methods.

Fig. 8: Rviz Visualization of Static Benchmarking (Hard Environment): One of the simulation runs in the hard static environment. The purple dot is the replanning point (Point A), the green polytopes are the safe flight corridors, and the color-coded trajectory is the optimized trajectory, where green is faster and red is slower. The orange dots are occupied voxels, the orange line is the original global path, and the red line is a smoother version of the global path used for SFC generation.

Fig. 9: Dynamic Benchmarking: The dynamic Gazebo environment used for benchmarking SANDO against state-of-the-art methods at three difficulty levels (Easy, Medium, and Hard)

N	VE	$R^{\mathrm{opt}}_{\mathrm{succ}}$	$T^{\mathrm{per}}_{\mathrm{opt}}$	$T_{\mathrm{trav}}$	$L_{\mathrm{path}}$	$S_{\mathrm{jerk}}$	$\rho_{\mathrm{viol}}$
N	VE	[%]	[ms]	[s]	[m]	[m/s 2 ]	[%]
4	Yes	93.4	1.3	13.2	6.8	1.4	0.0
4	No	93.4	9.6	13.2	6.8	1.4	0.0
5	Yes	100.0	6.8	11.1	6.8	1.9	0.0
5	No	100.0	12.8	11.1	6.8	1.9	0.0
6	Yes	100.0	17.2	9.8	6.8	2.8	0.0

TABLE IX: Variable elimination (VE) benchmarking results in standardized environment. We highlight the best and worst values for each

N

in green and red , respectively.

Fig. 10: Holybro PX4 Development Kit X500 is equipped with a Pixhawk flight controller and protective propeller guards. A Livox Mid-360 LiDAR is mounted on top for perception and localization.

Fig. 11: Static environment experiment (Experiment 3): the UAV successfully navigates through a cluttered environment with static obstacles. The agent’s speed is color-coded (green for faster, red for slower), and the point cloud is colored by height.

Exp.	$v_{\max}$ [m/s]	Computation Time [ms]
Exp.	$v_{\max}$ [m/s]	$T_{\mathrm{replan}}$	$T_{\mathrm{global}}$	$T_{\mathrm{SSFC}}$	$T_{\mathrm{opt}}$
1	1.0	$35.8\pm 8.8$	$0.1\pm 0.1$	$6.1\pm 3.0$	$14.0\pm 6.3$
2	2.0	$35.7\pm 8.8$	$0.1\pm 0.1$	$6.5\pm 3.2$	$12.3\pm 6.4$
3	3.0	$34.5\pm 7.6$	$0.1\pm 0.1$	$7.7\pm 3.2$	$9.7\pm 5.2$
4	4.0	$34.8\pm 8.0$	$0.1\pm 0.1$	$8.9\pm 4.0$	$8.6\pm 4.2$
5	5.0	$33.9\pm 7.0$	$0.1\pm 0.1$	$7.9\pm 3.6$	$8.3\pm 4.0$

TABLE XI: Hardware flight computation times in static environments with increasing velocity limits (constraint settings in the text) .

Fig. 13: Experiment 16: the UAV flies at $v_{\max}=2.0$ m/s through an $8\,\text{m}\times 20\,\text{m}$ area with five dynamic obstacles and additional static obstacles. The detected dynamic obstacles are visualized as boxes, and the future predicted trajectories of the dynamic obstacles are shown as a line.

Exp.	Obst. Traj.	Computation Time [ms]				Performance
Exp.	Obst. Traj.	$T_{\mathrm{replan}}$	$T_{\mathrm{global}}$	$T_{\mathrm{STSFC}}$	$T_{\mathrm{opt}}$	$\sigma_{\mathrm{track}}$ [m]	$f_{\mathrm{fail}}$ [%]	$d_{\min}$ [m]	$e_{\mathrm{est}}$ [m]
7	Line	$18.6$	$0.1$	$4.9$	$4.6$	0.32	5.9	2.01	0.30
8	Circle	$18.4$	$0.1$	$4.8$	$4.6$	0.30	3.0	1.51	0.29
9	Fig. Eight	$19.1$	$0.1$	$4.9$	$4.9$	0.27	12.4	1.59	0.30
10	Person	$18.9$	$0.1$	$5.0$	$4.7$	0.42	6.4	1.33	0.43

TABLE XII: Computation times and safety metrics for the single-dynamic-obstacle flights for Experiments 7–10; metric definitions in the text.

Exp.	Obst. Type	$v_{\max}$ [m/s]	Computation Time [ms]				Performance
Exp.	Obst. Type	$v_{\max}$ [m/s]	$T_{\mathrm{replan}}$	$T_{\mathrm{global}}$	$T_{\mathrm{STSFC}}$	$T_{\mathrm{opt}}$	$\sigma_{\mathrm{track}}$ [m]	$f_{\mathrm{fail}}$ [%]	$T^{\max}_{\mathrm{fail}}$ [ms]	$\hat{d}_{\min}$ [m]
11	5 Dyn.	2.0	$22.1$	$0.1$	$5.3$	$4.7$	0.28	21.5	677	0.34
12			$22.4$	$0.2$	$5.3$	$5.2$	0.30	13.2	700	1.04
13			$21.2$	$0.1$	$4.8$	$4.7$	0.28	0.4	44	1.50
14	5 Dyn. & Static	2.0	$20.8$	$0.1$	$5.3$	$4.2$	0.28	9.6	365	2.23
15			$21.1$	$0.1$	$5.0$	$4.9$	0.39	7.9	662	1.27

TABLE XIII: Computation times and performance metrics for the multi-dynamic-obstacle flights (Experiments 11–18) in the cluttered

8\,\mathrm{m}\times 20\,\mathrm{m}

space.

T^{\max}_{\mathrm{fail}}

is the longest run of consecutive replanning failures, and

\hat{d}_{\min}

the minimum center-to-center agent-to-obstacle distance as estimated onboard (not ground truth; see Section X ). Remaining metric definitions in the text.

Safe Motion Planning

Safe motion planning develops methods for robots and vehicles to reach goals while avoiding collisions and respecting dynamic and uncertainty constraints.

See all 215 papers

Oct 16, 2025

STITCHER: Constrained Trajectory Planning in Complex Environments with Real-Time Motion Primitive Search

Autonomous high-speed navigation through large, complex environments requires real-time generation of agile trajectories that are dynamically feasible, collision-free, and satisfy state or actuator constraints. Modern trajectory planning techniques primarily use numerical optimization, as they enable the systematic computation of high-quality, expressive trajectories that satisfy various constraints. However, stringent requirements on computation time and the risk of numerical instability can limit the use of optimization-based planners in safety-critical scenarios. This work presents an optimization-free planning framework called STITCHER that stitches short trajectory segments together with graph search to compute long-range, expressive, and near-optimal trajectories in real-time. STITCHER outperforms modern optimization-based planners through our innovative planning architecture and several algorithmic developments that make real-time planning possible. Extensive simulation testing is performed to analyze the algorithmic components that make up STITCHER, along with a thorough comparison with three state-of-the-art optimization planners. Simulation tests show that safe trajectories can be created within a few milliseconds for paths that span the entirety of two 50 m x 50 m environments. Hardware tests with a custom quadrotor verify that STITCHER can produce trackable paths in real-time while respecting nonconvex constraints, such as limits on tilt angle and motor forces, with flight speeds up to 63 km/h.

Helene J. Levy, Brett T. Lopez

VECTR Laboratory, University of California, Los Angeles, Los Angeles, CA, USA

Constrained Motion Planning Trajectory Generation UAV Navigation Graph-Based Planning Safe Motion Planning Motion Planning+1

Ask PDF

Oct 7, 2026

MeshSIPP: Efficient Lattice Planning in Dynamic Environment

Autonomous navigation in dynamic environments requires computing spatiotemporal trajectories that satisfy non-holonomic motion constraints. When the trajectories of the moving obstacles are predictable or known, a promising approach is to rely on the combination of state lattices constructed from precomputed feasible motion primitives and Safe Interval Path Planning -- a search-based algorithm with strong theoretical guarantees. While this approach yields feasible paths, the rich primitive sets needed for smooth navigation induce a large branching factor, which becomes costly when coupled with time-dependent obstacle intervals. To this end, we present MeshSIPP, an efficient planner that removes the computational bottleneck by exploiting the fact that many primitives sweep the same regions and can therefore be validated together. MeshSIPP propagates primitives as spatial bundles, screens them with lightweight bounding-interval checks, and defers the expensive exact departure-time search until a primitive reaches its terminal state. A time-aware pruning rule additionally discards redundant space-time branches early in the search. We prove that the resulting search is complete and optimal. Extensive experiments over more than 6,000 benchmark instances and real-time ROS~2 simulations show that MeshSIPP achieves up to a 3

\times

speedup over state-of-the-art spatiotemporal planners.

Marat Agranovskiy, Konstantin Yakovlev

Safe Interval Path Planning Motion Planning Robot Motion Planning Safe Motion Planning Dynamic Obstacle Avoidance

Ask PDF

Apr 30, 2026

Dynamic-TD3: A Novel Algorithm for UAV Path Planning with Dynamic Obstacle Trajectory Prediction

Deep reinforcement learning (DRL) finds extensive application in autonomous drone navigation within complex, high-risk environments. However, its practical deployment faces a safety-exploration dilemma: soft penalty mechanisms encourage risky trial-and-error, while most constraint-based methods suffer degraded performance under sensor noise and intent uncertainty. We propose Dynamic-TD3, a physically enhanced framework that enforces strict safety constraints while maintaining maneuverability by modeling navigation as a Constrained Markov Decision Process (CMDP). This framework integrates an Adaptive Trajectory Relational Evolution Mechanism (ATREM) to capture long-range intentions and employs a Physically Aware Gated Kalman Filter (PAG-KF) to mitigate non-stationary observation noise. The resulting state representation drives a dual-criterion policy that balances mission efficiency against hard safety constraints via Lagrangian relaxation. In experiments with aggressive dynamic threats, this approach demonstrates superior collision avoidance performance, reduced energy consumption, and smoother flight trajectories.

Wentao Chen, Jingtang Chen, Mingjian Fu +4

Reinforcement Learning UAV Navigation Constrained RL Dynamic Obstacle Avoidance Trajectory Prediction

Ask PDF

Abstract

Safe Motion Planning

Explore similar work

STITCHER: Constrained Trajectory Planning in Complex Environments with Real-Time Motion Primitive Search

MeshSIPP: Efficient Lattice Planning in Dynamic Environment

Dynamic-TD3: A Novel Algorithm for UAV Path Planning with Dynamic Obstacle Trajectory Prediction