SANDO: Safe Autonomous Trajectory Planning for Dynamic Unknown Environments

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

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.

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.

SANDO: Safe Autonomous Trajectory Planning for Dynamic Unknown Environments

Abstract

Figures & tables

Safe Motion Planning

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

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