RAPTOR: Robust and Perception-aware Trajectory Replanning for Quadrotor Fast Flight

Boyu Zhou; Jie Pan; Fei Gao; Shaojie Shen

Source-linked AI summary

RAPTOR: Robust and Perception-aware Trajectory Replanning for Quadrotor Fast Flight

Boyu Zhou, Jie Pan, Fei Gao, Shaojie Shen

arXiv:2007.03465v1Jul 6, 2020cs.RO

TL;DR

RAPTOR addresses the difficulty of rapidly generating feasible, high-quality trajectories for safe quadrotor flight in unknown environments while accounting for perception. It combines path-guided optimization over multiple topological paths with risk-aware refinement and active yaw planning to observe and avoid unknown obstacles. Extensive benchmark, simulation, and real-world evaluations report improved fast-flight performance and safety across several aspects.

Problem
High-speed navigation in unknown environments challenges existing replanners because limited computation time weakens feasibility and quality guarantees, while perception is often not considered.
Method
RAPTOR combines path-guided gradient-based optimization over multiple topological paths with risk-aware visibility and reaction-distance refinement and two-step active yaw planning.
Results
Extensive evaluations report that the planner outperforms state-of-the-art methods in several fast-flight aspects, while perception-aware planning supports fast and safe flight in challenging scenarios.
Takeaways & Limitations
The framework integrates robust replanning with active perception so quadrotors can observe potentially dangerous unknown regions earlier and act before collision.

Abstract

from arXiv · show

Recent advances in trajectory replanning have enabled quadrotor to navigate autonomously in unknown environments. However, high-speed navigation still remains a significant challenge. Given very limited time, existing methods have no strong guarantee on the feasibility or quality of the solutions. Moreover, most methods do not consider environment perception, which is the key bottleneck to fast flight. In this paper, we present RAPTOR, a robust and perception-aware replanning framework to support fast and safe flight. A path-guided optimization (PGO) approach that incorporates multiple topological paths is devised, to ensure finding feasible and high-quality trajectories in very limited time. We also introduce a perception-aware planning strategy to actively observe and avoid unknown obstacles. A risk-aware trajectory refinement ensures that unknown obstacles which may endanger the quadrotor can be observed earlier and avoid in time. The motion of yaw angle is planned to actively explore the surrounding space that is relevant for safe navigation. The proposed methods are tested extensively. We will release our implementation as an open-source package for the community.

I. INTRODUCTION

RAPTOR targets high-speed quadrotor flight in unknown, cluttered environments, where replanning must produce feasible trajectories quickly while accounting for limited perception. It combines topological path guidance, perception-aware refinement, and active yaw planning, with evaluations in simulation and real-world experiments.

High-speed flight in unknown, cluttered environments requires rapid trajectory replanning to avoid previously unknown obstacles while maintaining smooth and safe navigation.
Existing methods may fail to find feasible trajectories within limited time, remain confined to unsatisfactory local topological classes, and neglect perception constraints.
RAPTOR uses path-guided gradient-based optimization with multiple topologically distinctive paths to explore the solution space and generate feasible, high-quality trajectories.
Its risk-aware refinement explicitly enforces visibility and safe reaction distance around potentially dangerous unknown regions.
A two-step yaw-angle planner actively explores unknown regions relevant to flight, while extensive simulation and real-world tests evaluate the complete system.

C. Navigation in Unknown Environments

RAPTOR combines robust replanning with perception awareness for unknown, cluttered environments. Its pipeline uses path-guided optimization, risk-aware refinement, and yaw planning to improve safety and visibility during fast flight.

Perception-aware planning: Existing strategies either treat unknown space optimistically or restrict motion to known-free regions, while lacking environment-perception awareness needed for fast flight.Optimistic planning may not guarantee safety, whereas conservative planning can restrict flight speed.
System overview: The replanning system deforms a global reference trajectory locally, first generating multiple locally optimal trajectories through path-guided optimization.The optimization is guided by topologically distinctive paths extracted through sampling-based searching.
Perception-aware planning: Risk-aware refinement improves the selected trajectory’s safety and visibility to unknown dangerous space, after which yaw planning actively explores relevant unknown environments.The refinement enforces visibility and safe reaction distance to unknown obstacles, improving predictability and safety in fast flights.
Path-guided optimization: ESDF-only optimization can fail near valleys and ridges where distance gradients change abruptly or push adjacent trajectory parts in opposing directions.Additional information is therefore introduced to produce gradients that consistently deform trajectories toward free space.
Path-guided optimization: PGO uses a geometric guiding path to attract an initial B-spline trajectory toward free space before ESDF-based optimization refines it.The first phase produces a warmup trajectory near the guiding path, and the second phase further optimizes it for smoothness, safety, and dynamic feasibility.
Path-guided optimization: The first PGO phase takes negligible time and generates a warmup trajectory that enables better trajectories within shorter overall time.The warmup trajectory is easier to refine because its ESDF gradients vary more smoothly.

V. TOPOLOGICAL PATH SEARCHING

RAPTOR searches multiple distinctive topological guiding paths because a single locally optimized trajectory may remain trapped in an unsatisfactory topological class. Its sampling-based search is redesigned for real-time operation in complex 3D environments.

Motivation: A trajectory optimized from one guiding path remains within its topologically equivalent class, which may not contain a satisfactory smooth and safe solution.Selecting the best geometric path is difficult because geometric paths omit higher-order motion information, while kinodynamic search can be too slow.
Topological path searching: Sampling-based topological path searching produces a collection of distinctive guiding paths to improve the chance of finding a better trajectory.The algorithm is redesigned to operate in real time in complex 3D environments.

A. Topology Equivalence Relation

RAPTOR defines uniform visibility deformation (UVD) to group collision-free corresponding trajectories while retaining more useful 3D path distinctions than homotopy. UVD is designed to make equivalence checking substantially more efficient than visibility deformation.

Motivation: UVD captures abundant useful trajectories in 3D environments, whereas homotopy captures insufficient useful trajectories for this setting.
Definition: UVD places two endpoint-matched trajectories in the same class when corresponding points can be connected by collision-free straight lines.The correspondence uses the same parameter s on both trajectories.
Comparison: Unlike visibility deformation, UVD maps points with equal parameters and captures slightly more distinct path classes.
Efficiency: UVD is less expensive to check than visibility deformation, which requires computing a visibility diagram and searching paths within it.
Testing: UVD equivalence can be tested by uniformly discretizing s and checking collision along corresponding connecting line segments.For piecewise straight paths, the parameterization is made uniform.

B. Topological Roadmap

The topological roadmap constructs a compact set of paths across distinct UVD classes, then shortens and prunes them before using them to guide parallel trajectory optimization. This limits redundancy and computational complexity while preserving diverse candidate routes.

Roadmap construction: The roadmap uses guards to explore uncovered free-space regions and connectors to form paths between guards.The start and goal are initialized as guards, while newly sampled invisible points become additional guards.
Path extraction: A depth-first search with a visited-node list extracts paths between the start and goal from the UVD roadmap.
Path shortening: Shortcut paths replace detoured roadmap paths because excessive deformation during path-guided optimization can produce unsmooth trajectories.
Path shortening & pruning: The roadmap illustration shows equivalent paths grouped by color, pruned paths as dashed segments, and shortened paths guiding parallel PGOs.The red trajectory is shown as smoothest despite having a longer guiding path, because it takes fewer turns.
Pruning: Only topologically distinct paths are retained, while the first Kmax shortest paths and paths exceeding rmax times the shortest length are selected or pruned.These rules bound complexity and are stated not to miss a potentially optimal solution.

8 Ps.push back(Pd.back())

Risk-aware refinement checks where an optimistic trajectory enters unknown space and modifies it so relevant frontier points become visible earlier with sufficient reaction distance. The procedure iteratively enforces visibility and safety constraints on the selected trajectory.

Refinement: Refinement begins by checking the optimistic trajectory’s visibility status and then enforcing visibility to relevant unknown space and safe reaction distance.
Visibility status: The visibility status records tf, pf, tc, pc, and vc, which describe the first unknown point, its timing, critical viewing position, and viewing direction.
Frontier point: The first unknown point pf is prioritized because it lies on a promising goal-directed trajectory, may be dangerous, and is reached earlier than later unknown points.
Visibility constraint: Visibility is considered reliable when a preceding position reaches the required visibility level ψmin for pf.The visibility level is based on the minimum signed distance between the viewing segment and obstacles, queried from the ESDF.
Risk-aware refinement: If pf is not visible early enough, refinement shifts the trajectory toward a critical viewing direction and increases the distance available for collision avoidance.The figure describes pf becoming viewable earlier along the refined trajectory.

1) Worst-case Safety Criteria:

The safety refinement handles the worst case of an obstacle appearing immediately behind the first unknown point by requiring earlier visibility and enough stopping distance. These requirements are incorporated as soft penalties and enforced through an iterative speed-estimation procedure.

Worst-case safety criteria: At the critical viewing position, the quadrotor must have enough distance to decelerate to a stop before an obstacle directly behind pf, accounting for vehicle size and disturbance.
View constraint: When the initial trajectory reveals pf too late, the refined trajectory must reach the critical viewing ray at an earlier time ts < tc.
Iterative refinement: Because the refined speed is initially unknown, the method estimates it, refines the trajectory, then increases the estimate by α until the safety criteria are satisfied.The procedure is stated to terminate quickly in practice because speed varies slightly along the smooth trajectory.
Optimization: View and safety requirements enter gradient-based optimization through penalty functions and are combined with the existing trajectory cost.

VII. YAW ANGLE PLANNING

RAPTOR plans yaw angles to actively observe relevant unknown space while balancing information gain and yaw smoothness.

VII. YAW ANGLE PLANNING: Yaw-angle planning decomposes into graph search followed by trajectory optimization.The graph search selects yaw angles along the refined position trajectory, and optimization produces a smooth, dynamically feasible yaw trajectory.
VII. YAW ANGLE PLANNING: The searched yaw-angle sequence trades off smoothness against information gain from unknown space.The weighting parameter µ adjusts the contribution of smoothness.
VII. YAW ANGLE PLANNING: Yaw samples are selected at uniformly distributed positions along the refined trajectory, with the initial yaw fixed by the current quadrotor state.

2) Information Gain:

The perception-aware planner estimates information gain efficiently, prioritizes unknown space relevant to the trajectory, and optimizes a dynamically feasible yaw profile.

2) Information Gain:: Potential information gain counts unmapped voxels that fit the camera model and remain visible without occupied-voxel blockage.The evaluation distinguishes visible unknown voxels from blocked ones and stores visibility in overlapping fields of view for later checks.
2) Information Gain:: FOV voxel subsampling, parallel gain evaluation, and cached raycasting reduce the cost of online information-gain computation.These adaptations approximate gain with slight error while reducing runtime and avoiding repeated visibility checks.
2) Information Gain:: Information gain weights unknown voxels more heavily when they are closer laterally or longitudinally to the trajectory and current position.This biases sensing toward space more relevant to point-to-point flight rather than uniform environmental coverage.
2) Information Gain:: Yaw optimization fits a uniform B-spline through the searched yaw angles while enforcing smoothness, waypoint tracking, and soft dynamic-feasibility constraints.The objective includes smoothness, waypoint, angular-velocity, and angular-acceleration terms.
2) Information Gain:: The B-spline convex-hull property guarantees yaw-trajectory feasibility when control points remain within angular-velocity and acceleration limits.
2) Information Gain:: Experiments use a RealSense D435 quadrotor platform and an Intel Core i7-8550U CPU, with volumetric mapping and ESDF support for optimization and visibility evaluation.The evaluation focuses on local replanning with naive global reference trajectories such as straight lines.

3) State Estimation and Control:

The experiments evaluate active perception in real-world scenes using visual-inertial state estimation, mapping, planning, and control, showing earlier obstacle discovery and safer reactions with RAPTOR.

3) State Estimation and Control:: Real-world tests use robust visual-inertial localization, while simulation uses ground-truth odometry from the quadrotor dynamics model.A geometric controller tracks both position and yaw trajectories.
3) State Estimation and Control:: Four local planners are tested three times in each of two scenes at maximum velocity 3m/s and acceleration 2.5m/s2.An emergency stop is triggered when detected collision points are closer than 0.5m.
3) State Estimation and Control:: In scene 1, risk-aware planners improve visibility behind obstacles, while active exploration yaw discovers occluded boxes earlier than velocity-tracking yaw.Velocity-tracking yaw causes one failure, whereas active exploration yaw enables earlier action.
3) State Estimation and Control:: In scene 2, only planner D safely reaches the goal because other planners collide with an obstacle hidden behind a corner.Emergency stopping does not provide enough time to avoid the collision.
3) State Estimation and Control:: The experiments identify visibility toward flight-relevant unknown regions and active viewing of relevant directions as critical factors for high-speed flight safety.

2) Benchmark Comparisons:

Benchmark experiments compare RAPTOR with exploration and state-of-the-art replanners across safety, efficiency, and aggressive-flight settings, reporting stronger overall performance with competitive computation efficiency.

2) Benchmark Comparisons:: Compared with SLE, RAPTOR achieves more successful flights in increasingly cluttered scenes by enforcing visibility toward dangerous unknown areas and actively controlling yaw.SLE’s velocity-tracking yaw can fail to face dangerous unknown regions in time.
2) Benchmark Comparisons:: RAPTOR reduces flight distance and time relative to SLE by focusing on flight-relevant unknown areas instead of taking information-gathering detours.SLE plans only through known unoccupied space and selects intermediate goals partly by information gain.
2) Benchmark Comparisons:: Against FASTER, EWOK, and RE Traj., RAPTOR outperforms them in flight distance, flight time, and energy consumption with competitive computation efficiency.FASTER has higher overhead from MIQP, while EWOK and RE Traj. have failure or safety issues in dense environments.
2) Benchmark Comparisons:: Aggressive-flight experiments use dense indoor scenes with neighboring obstacles around 1 meter apart and a 7m local-replanning horizon.
2) Benchmark Comparisons:: One aggressive flight reaches a maximum speed of 2.90 m/s and average speed of 1.77 m/s over 14.12 m in 8.0 s.

3) Outdoor Flight Test:

RAPTOR was evaluated in challenging outdoor forests, where the quadrotor executed autonomous flights through irregular environments without external localization. The tests included slope traversal and flights to goals up to 45 m away.

Outdoor flight tests: The planner was tested in three outdoor forest scenes requiring agile three-dimensional maneuvers around rocks, branches, and leaves.The experiments used natural, unstructured, irregular environments and did not rely on external localization devices.
Forest 1: In the first forest, the quadrotor flew 39 m to its goal at a maximum speed of 3.19 m/s and an average speed of 2.29 m/s.The flight covered 40.78 m in 17.83 s.
Forest 2: In the second forest, the quadrotor flew to two goals, first traversing 30 m with a 7 m height change and then continuing 17 m to the second goal.The complete flight covered 48.71 m in 23.19 s.
Forest 3: In the third forest, the quadrotor reached a goal 45 m away over a 46.85 m flight at a maximum speed of 3.41 m/s and an average speed of 2.14 m/s.The flight took 21.91 s to finish.
Recorded outputs: Online maps and executed trajectories were recorded for the forest experiments, with map height represented by color.Velocity profiles included both indoor and forest sample flights.