PaperlayerView exact revision on arXiv

Source-linked AI summary

FASTER: Fast and Safe Trajectory Planner for Navigation in Unknown Environments

Jesus Tordesillas, Brett T. Lopez, Michael Everett, Jonathan P. How

arXiv:2001.04420v2cs.ROcs.CV

TL;DR

High-speed UAV navigation in unknown environments must balance rapid motion with safety under partial observability. FASTER plans in free-known and unknown space while maintaining a safe backup trajectory in free-known space, and its MIQP lets the solver choose interval allocation. Experiments reached 7.8 m/s for UAVs and 2 m/s for a skid-steer robot.

  • Problem

    High-speed UAV navigation requires real-time collision-free, dynamically feasible trajectories under partial observability, while free-known-space stop conditions can limit speed.

  • Method

    FASTER optimizes Whole and Safe trajectories, planning in F ∪ U while maintaining a Safe Trajectory in F and allowing solver-selected interval allocation.

  • Results

    7.8 m/s was achieved in hardware UAV experiments, and skid-steer robot experiments reached 2 m/s.

  • Takeaways & Limitations

    FASTER provides a planning framework that maintains safety without reducing nominal flight speed and supports both UAV and skid-steer hardware experiments.

  • Takeaways & Limitations

    For large known worlds, long planning horizons may require more than four polyhedra, increasing computation time.

Abstract

from arXiv · show

Planning high-speed trajectories for UAVs in unknown environments requires algorithmic techniques that enable fast reaction times to guarantee safety as more information about the environment becomes available. The standard approaches that ensure safety by enforcing a "stop" condition in the free-known space can severely limit the speed of the vehicle, especially in situations where much of the world is unknown. Moreover, the ad-hoc time and interval allocation scheme usually imposed on the trajectory also leads to conservative and slower trajectories. This work proposes FASTER (Fast and Safe Trajectory Planner) to ensure safety without sacrificing speed. FASTER obtains high-speed trajectories by enabling the local planner to optimize in both the free-known and unknown spaces. Safety is ensured by always having a safe back-up trajectory in the free-known space. The MIQP formulation proposed also allows the solver to choose the trajectory interval allocation. FASTER is tested extensively in simulation and in real hardware, showing flights in unknown cluttered environments with velocities up to 7.8m/s, and experiments at the maximum speed of a skid-steer ground robot (2m/s).

I. INTRODUCTION

FASTER addresses the speed–safety tradeoff in unknown environments by planning in known-free and unknown space while maintaining a safe backup trajectory. Its flexible interval allocation and experiments target faster, less conservative planning.

  • High-speed UAV navigation must repeatedly generate collision-free, dynamically feasible trajectories under partial observability and limited sensing.
  • Planning only in free-known space with a final stop condition can limit speed, especially when free-known space is small relative to occupied-known and unknown space.
  • The solver chooses interval allocation, reducing conservatism associated with forcing one interval per polyhedron and avoiding ad-hoc time allocation.
  • FASTER plans a Whole Trajectory in F ∪ U and maintains a Safe Trajectory in F, executing the backup only when subsequent replanning becomes infeasible.
  • FASTER adds a cheap cost-to-go heuristic for selecting the optimization direction and provides a feasibility theorem guaranteeing safety.
  • 7.8 m/s was achieved in unknown cluttered environments, while skid-steer hardware experiments reached 2 m/s.

II. RELATED WORK

Prior UAV planners differ in optimization formulation, planning space, mapping strategy, and planner–mapper integration. FASTER is situated among methods using map-based planning and convex environmental representations while addressing their stated limitations.

  • State-of-the-art UAV planning methods are classified by optimization formulation and the local planner’s operating space.
  • Search-based and motion-primitive methods can be limited to short trajectories and may be unable to perform complex obstacle maneuvers.
  • Optimization-based approaches use obstacle penalties, successive convexification, or convex decompositions, with reported computational, initialization, or feasibility limitations.
  • Local planners may use instantaneous perception or historical maps; planners optimizing only inside F require a final stop condition for safety.
  • Mapping methods are memory-less or fusion-based, with fusion-based methods commonly using occupancy grids or distance fields but facing estimation-error and fusion-time drawbacks.
  • FASTER uses a body-centered sliding occupancy-grid map, fuses depth data with 3-D Bresenham ray tracing, and inflates O and U by the UAV radius.

B. Global Planner

FASTER combines JPS-based global planning with convex decomposition and MIQP trajectory optimization. Its local planner uses jerk-controlled polynomial trajectories, preserves a safe backup, and adapts time allocation while allowing interval allocation to be optimized.

  • Global path generation: JPS finds a shortest piece-wise linear path from the UAV to the goal while avoiding known obstacles.JPS is used instead of A* because it runs an order of magnitude faster while retaining completeness and optimality.
  • Global path generation: Convex decomposition constructs polyhedra around segments of the JPS path by inflating aligned ellipsoids and computing obstacle-contact tangent planes.
  • Trajectory types: The Whole Trajectory lies in F ∪U and has a final stop condition, whereas the Safe Trajectory runs from R to F entirely inside F with a final stop condition.The Committed Trajectory concatenates the A →R portion of the Whole Trajectory with the Safe Trajectory.
  • Trajectory parameterization: The local planner models each interval as a cubic polynomial generated by constant jerk, then expresses the trajectory through Bézier control points.The trajectory is divided into N intervals with common duration dt, and each interval can be represented as a cubic Bézier curve.
  • MIQP formulation: Binary variables assign every interval to at least one polyhedron, allowing the MIQP optimizer to choose the interval allocation.Because Bézier curves lie inside the convex hull of their control points, constraining control points to a polyhedron keeps the interval inside that corridor.
  • Time allocation: The interval duration is computed from motion-time lower bounds and a factor f≥1 adapted around the factor that worked at the previous replanning step.The search tries increasing factor values until the optimization becomes feasible, while seeking small dt and few trials.

E. Complete Algorithm

At each replanning step, FASTER selects a direction, optimizes a Whole Trajectory through known and unknown space, and constructs a Safe Trajectory in known free space. It retains the previous committed trajectory when replanning is infeasible, unsafe, or too slow.

  • Replanning initialization: The algorithm chooses point A on the previous committed trajectory using an offset from the UAV’s current position, then projects the goal into the sliding map and runs JPS from A to G.The offset factor α is dynamically adjusted so the solver can usually find the next solution within the available replanning time.
  • Direction selection: FASTER compares the new JPS path with a modified previous path that avoids newly observed obstacles, selecting the direction with the lower estimated cost-to-go.The comparison is made only when the path-change angle exceeds a threshold, typically 15°.
  • Whole trajectory: The Whole Trajectory is obtained by convexly decomposing U ∪F around the selected JPS segment and solving an MIQP over the resulting overlapping polyhedra.
  • Safe trajectory: The Safe Trajectory begins at R near the Whole Trajectory’s intersection with unknown space and is optimized entirely inside known free space F.R is chosen as the nearest state to the intersection that is not in inevitable collision with U, using a stopping-distance heuristic.
  • Heuristic limitation: The stopping-distance heuristic may be conservative because it decouples x and y axes and approximates the system as a double integrator; z is ignored to reduce conservativeness.
  • Committed trajectory: The Committed Trajectory concatenates WholeA→R with Safe, keeping the UAV on a trajectory guaranteed to remain inside F if later replanning fails.The two trajectory optimizations are decoupled so braking does not influence the nominal-speed segment A →R.
  • Fallback behavior: The previous committed trajectory continues executing if either optimization is infeasible, A−R intersects U, or replanning exceeds δt.

F. Feasibility Theorem

Under a noise-free, static-world assumption, FASTER guarantees that every committed trajectory remains within free space, including free-unknown space. The proof uses induction and remains valid for sliding, nonsliding, and instantaneous-field-of-view maps.

  • Feasibility guarantee: FASTER guarantees every committed trajectory is contained in free space, whether that space is known or unknown.The feasibility theorem states this safety property for all replanning steps.
  • Assumption: The theorem assumes that the map is noise-free and the world is static, so free space does not change between replanning steps.Formally, F_k ∪ FUnknown,k = F_k+1 ∪ FUnknown,k+1 for all k.
  • Proof: The inductive proof establishes safety initially from the checked trajectory segment and safe trajectory, then preserves it by reusing or newly constructing a safe committed trajectory.When scenarios 1–3 occur, the algorithm retains the prior committed trajectory; otherwise, it constructs the new trajectory inside F_k+1.
  • Scope: The guarantee does not require free-known space to grow and is independent of map shape or retained history length.Thus, it also applies to a nonsliding global map and a map containing only the sensor field of view.
  • Scope: Reusing a committed trajectory can preserve free-space information outside the current sliding map, effectively embedding that information in the trajectory.This is described as a form of data compression.

G. Controller

FASTER tracks its planned trajectory with a cascade controller that orients the UAV camera toward the intersection of the JPS path and unknown space. Hardware state estimation fuses IMU propagation with external motion capture.

  • Controller: A cascade controller tracks FASTER trajectories, while UAV yaw is selected to point the camera toward M, the intersection of JPS_k and U.The same controller is used in all UAV simulation and hardware experiments.
  • State estimation: In real-hardware experiments, position, velocity, attitude, and IMU biases are estimated by fusing propagated IMU measurements with external motion capture.

A. Forest, bugtrap and office simulations

FASTER is evaluated in simulated forest, bugtrap, and office environments using depth-camera perception, with comparisons showing shorter completion distances or times than prior approaches. Timing analyses characterize MIQP, convex-decomposition, and JPS runtimes.

  • Experimental setup: The simulations use custom C++ nonlinear dynamics integrated with Runge–Kutta and Gazebo depth perception, with a 90° horizontal field of view.The sensing range is 5 m in the corner environment and 10 m in the remaining simulations.
  • Forest simulation: 8–51% improvement in total distance flown is achieved by FASTER across 10 random forest simulations compared with the evaluated approaches.The comparison includes incremental, random-goal, NBVP, Safe Local Exploration, and prior work.
  • Forest simulation: 52% improvement in completion time is achieved by FASTER over prior work in the random forest simulations.Completion-time values were available only for among the compared methods.
  • Bugtrap simulation: 63% improvement in total flight time is achieved by FASTER in the bugtrap environment while both algorithms have similar total distance.The corresponding velocity profile is shown in Fig. 14.
  • Office simulation: 29.6% improvement in flight time is achieved by FASTER in the office environment despite flying slightly farther than prior work.FASTER entered one of the last rooms and then turned back; the office limits were v_max = 3 m/s, a_max = 6 m/s^2, and j_max = 35 m/s^3.
  • Runtime analysis: For forest experiments, MIQP safe-trajectory runtime is approximately constant as P_max changes because planning occurs only in free-known space.The experiments use P_max = 2–4; the whole and safe trajectories use 10 and 7 intervals, respectively.

B. Time vs. Interval allocation

FASTER optimizes interval allocation with binary variables while fixing equal per-interval times, contrasting with approaches that optimize time allocation under fixed interval assignments. In 50 runs, interval allocation succeeds consistently and yields lower total cost.

  • Allocation strategies: Time-allocation approaches optimize time while assigning N/P intervals per polyhedron, with total time either free or fixed at 12.5 s.
  • Allocation strategies: FASTER’s interval-allocation approach optimizes the allocation of N trajectory intervals using binary variables while assigning every interval the same fixed time.With T = 12.5 s, each interval receives 12.5/N seconds.
  • Evaluation: 50 runs compare the approaches in free space defined by four overlapping polyhedra, using a cost combining squared jerk and total trajectory time.The initial and final states are stop conditions in randomly selected and final polyhedra, respectively.
  • Results: FASTER’s interval-allocation method succeeds in all runs and obtains smaller total costs than the compared time-allocation methods.The interval-allocation MIQP uses Gurobi, whereas the nonconvex time-allocation problems use fmincon.

C. Role of the Safe Trajectory

The Safe Trajectory is crucial for maintaining safety at high speeds when newly discovered obstacles leave too little time to replan. FASTER also improves speed by planning partly through unknown space while retaining a free-known-space fallback.

  • Speed achieved: 6.02 m/s versus 5.06 m/s: FASTER is faster than planning only in free-known space on the A →R segment.Both planners guaranteed safety in this comparison; the initial velocity at A was 4.8 m/s.
  • Trajectory roles: The Safe Trajectory remains in free-known space, while the Whole Trajectory can extend through unknown space toward the global plan.Only a portion of the Whole Trajectory is implemented; the Safe Trajectory is available at each replanning step as the fallback.
  • Safety: 6 m/s and above: omitting the Safe Trajectory causes collisions when newly discovered obstacles leave insufficient replanning time.At speeds of 4 m/s or below, the Safe Trajectory was not strictly necessary in the tested corner environment.
  • Safety: The corner test uses an obstacle behind the corner, creating a setting where hidden obstacles are revealed only after the UAV turns.The environment is specifically used to evaluate the consequences of committing directly to the Whole Trajectory.

D. Comparison between PolyWhole and PolySafe

PolyWhole covers substantially more unknown space than PolySafe, extending the planning horizon beyond the free-known region. This broader coverage supports higher nominal speed while the hardware experiments demonstrate operation across cluttered environments.

  • Volume comparison: 145.1·VUAV: PolyWhole covers this unknown volume beyond PolySafe in the four-polyhedra corner environment.Here VUAV is the volume of a sphere with radius 0.3 m modeling the UAV.
  • Volume comparison: 250.8·VUAV and 21.9·VUAV: PolyWhole is respectively larger than PolySafe in the office and forest simulations.PolyWhole covers unknown volumes of 122.8·VUAV and 5.5·VUAV in those simulations, while PolySafe covers none.
  • Volume comparison: Hundreds of UAV volumes: PolyWhole can cover unknown space far beyond PolySafe, especially in cluttered environments.PolySafe is restricted to free-known space, whereas PolyWhole extends through the union of unknown and free-known space.
  • Hardware experiments: 7.8 m/s: the maximum speed achieved in the sixth hardware experiment series was reached in Experiment 5.Experiments 1–2 reached 2.1 m/s, while Experiments 3–4 reached 3.6 m/s.
  • Measurement caveat: Velocity estimates are noisy at high speeds and unavailable below obstacles, affecting measurements in specified portions of Experiments 2 and 4.The estimates are obtained by finite differences of external motion-capture ground-truth positions.
  • Runtime: Below 32 ms: the 75th-percentile MIQP runtime for both Whole and Safe Trajectories in the reported timing experiments.Measurements used Pmax = 2 on an Intel Core i7-7700HQ for simulation and an onboard Intel NUC for hardware.

VI. EXTENSION TO A GROUND ROBOT

FASTER extends to skid-steer robots by generating 2-D trajectories and adapting the controller. Ground-robot experiments reached approximately the robot’s specified maximum speed, including successful bugtrap exit and map reuse.

  • Extension method: 2-D trajectories and a modified controller adapt FASTER to skid-steer robots.Linear and angular velocities are generated with a PD controller based on trajectory geometry and desired position and velocity.
  • Hardware experiments: 1.95 m/s and 2.22 m/s: Experiments 7 and 8 approached or exceeded the ground robot’s specified maximum speed of approximately 2 m/s.The experiments used obstacle environments similar to the random forest.
  • Bugtrap experiment: Experiment 9 demonstrates map reuse in a bugtrap: the robot enters, detects the dead end, turns back, exits, and reaches the goal around new obstacles.Only depth-image points within 3 m were used to build the map.
  • Limitations: Planning horizons requiring more than four polyhedra increase computation time in large known worlds.The reported experiments typically needed 2–4 polyhedra, preserving computational tractability.
  • Future work: Time-allocation approaches remain a future optimization target because the generic nonconvex solver may be inefficient in some situations.The paper suggests exploiting problem structure or hierarchical optimization to reduce computation time.
Loading 2001.04420v2…