PaperlayerView exact revision on arXiv

Source-linked AI summary

Teach-Repeat-Replan: A Complete and Robust System for Aggressive Flight in Complex Environments

Fei Gao, Luqi Wang, Boyu Zhou, Luxin Han, Jie Pan, Shaojie Shen

arXiv:1907.00520v1cs.RO

TL;DR

Poor teaching trajectories and changing environments limit conventional quadrotor teach-and-repeat. Teach-Repeat-Replan converts rough demonstrations into topology-equivalent safe trajectories and adds onboard local replanning, with aggressive indoor and outdoor flights validating the complete system.

  • Problem

    Poor or dynamically infeasible teaching trajectories, environmental changes, moving obstacles, and localization drift limit flexible and robust quadrotor teach-and-repeat.

  • Method

    Teach-Repeat-Replan preserves the teaching trajectory’s topology through a flight corridor, optimizes spatial and temporal trajectories, and adds local perception and replanning.

  • Results

    The system converts arbitrarily jerky or poor demonstrations into efficient and safe global trajectories and supports aggressive flights in complex environments.

  • Takeaways & Limitations

    The complete system captures users’ flight intentions while adapting to environmental changes, dynamic obstacles, and localization drift.

Abstract

from arXiv · show

In this paper, we propose a complete and robust motion planning system for the aggressive flight of autonomous quadrotors. The proposed method is built upon on a classical teach-and-repeat framework, which is widely adopted in infrastructure inspection, aerial transportation, and search-and-rescue. For these applications, human's intention is essential to decide the topological structure of the flight trajectory of the drone. However, poor teaching trajectories and changing environments prevent a simple teach-and-repeat system from being applied flexibly and robustly. In this paper, instead of commanding the drone to precisely follow a teaching trajectory, we propose a method to automatically convert a human-piloted trajectory, which can be arbitrarily jerky, to a topologically equivalent one. The generated trajectory is guaranteed to be smooth, safe, and kinodynamically feasible, with a human preferable aggressiveness. Also, to avoid unmapped or dynamic obstacles during flights, a sliding-windowed local perception and re-planning method are introduced to our system, to generate safe local trajectories onboard. We name our system as teach-repeat-replan. It can capture users' intention of a flight mission, convert an arbitrarily jerky teaching path to a smooth repeating trajectory, and generate safe local re-plans to avoid unmapped or moving obstacles. The proposed planning system is integrated into a complete autonomous quadrotor with global and local perception and localization sub-modules. Our system is validated by performing aggressive flights in challenging indoor/outdoor environments. We release all components in our quadrotor system as open-source ros-packages.

I. INTRODUCTION

The paper develops a complete teach-and-repeat system that converts rough human flight demonstrations into topologically equivalent aggressive trajectories while adapting to changing environments. It combines corridor-based global planning, onboard local perception and replanning, and a complete autonomous quadrotor architecture.

  • Motivation: Teach-and-repeat must capture human intention while handling poor demonstrations and changing environments that make precise repetition difficult.The paper identifies unskilled pilots, dynamically infeasible taught trajectories, environmental changes, and localization sensitivity as practical barriers.
  • Proposed approach: The system accepts an arbitrarily slow or jerky trajectory preserving the expected route topology, then generates a topologically equivalent, efficient trajectory with expected aggressiveness.The generated trajectory is not required to reproduce the human-piloted path exactly.
  • Local replanning: Sliding-windowed local perception and replanning produce safe trajectories that avoid unmapped or moving obstacles during repetition.The local modules also address environmental changes and localization drift.
  • Global planning: A flight corridor preserves the teaching trajectory’s topology, while spatial and temporal planning generate safe and physically feasible global trajectories.The system first reconstructs the environment, records the demonstrated trajectory, and optimizes spatial and temporal components within the corridor.
  • Scope and contribution: The paper extends classical teach-and-repeat beyond exact path following and releases its planning, perception, localization, and control components as open-source packages.The system is evaluated through experiments and comparisons in varied scenarios.
  • System architecture: The architecture places global mapping, planning, and visualization on a ground station, while state estimation, local sensing, and replanning run onboard.This division integrates global and local processing into the autonomous quadrotor system.

C. Global Spatial-Temporal Planning

The system preserves the teaching trajectory’s topology by grouping nearby free space into large convex polyhedrons, then optimizing safe spatial and physically feasible temporal trajectories within the resulting corridor.

  • Local Collision Avoidance: During repeating flight, local mapping and sliding-window replanning respond to localization drift, environmental changes, and moving obstacles.Local collision checks trigger gradient-based trajectory optimization when obstacles are detected.
  • Flight Corridor Generation: The flight corridor preserves the teaching trajectory’s topological structure while providing greater optimization freedom than axis-aligned cubes.The corridor contains the teaching trajectory and captures free space around it for trajectory optimization.
  • Flight Corridor Generation: General convex polyhedrons capture more free space than axis-aligned cubes, which can restrict optimization or fail to cover a teaching trajectory in narrow environments.The proposed method avoids assumptions about obstacle convexity and supports arbitrarily jerky teaching trajectories.
  • Convex Cluster Inflation: Convex cluster inflation grows a free voxel set around an arbitrary seed while retaining convexity through collision-free ray checks.Candidate voxels are added only when rays to existing cluster voxels do not pass through obstacles.
  • Polyhedron Representation: The clustered voxel set is converted into a convex-hull polyhedron and then into hyperplane constraints for spatial trajectory optimization.Quick hull produces a vertex representation, which is converted to an equivalent hyperplane representation.

B. CPU Acceleration

CPU acceleration makes convex cluster inflation practical by reducing redundant ray checks while preserving the convexity test’s correctness.

  • Computational Bottleneck: Ray-casting from every candidate voxel to all cluster voxels is too slow for real-time operation on fine-resolution occupancy maps.The algorithm’s all-voxel and all-ray iteration creates the computational bottleneck.
  • Initialization: Initializing clusters as rapidly inflated axis-aligned cubes provides a fast starting region before convex clustering expands the free space.Cube inflation requires only O(1) index queries for initialization.
  • Initialization: The initialization can produce a different final polyhedron from clustering from scratch because cubes expand only along x, y, and z directions.Convex clusters can grow through all 26-connected directions in a 3D grid.
  • Ray-Casting Reduction: Early termination at inner voxels is sufficient for convexity checking, allowing rays to stop once they reach voxels known to lie inside the convex cluster.This follows because line segments from inner voxels to other cluster voxels remain inside the convex set.
  • Ray-Casting Reduction: Checking only outer boundary voxels is sufficient for candidate convexity tests, further reducing the number of rays that must be cast.Rays from inner voxels to an external candidate must pass through an outer voxel first.
  • Result: The resulting convex cluster inflation runs in real time on CPUs at a 0.2m grid resolution.The effectiveness of these techniques is numerically validated in the paper’s benchmark section.

C. GPU Acceleration

GPU parallelization accelerates convex cluster inflation by processing candidate voxels and ray checks concurrently while preserving sequential conflict information.

  • GPU Acceleration: The GPU scheme speeds convex cluster inflation by one order of magnitude when a GPU is available.Parallel ray casting and collision checking exploit the algorithm’s many-core structure.
  • Parallel Inflation: The parallel implementation evaluates all neighboring candidates of the active voxel set in parallel during each iteration.This reduces serialized discovery and minimizes CPU-GPU data transfers.
  • Conflict Handling: Each GPU kernel checks candidate rays against existing cluster voxels and earlier-indexed candidates to detect conflicts between simultaneously examined voxels.A sequential-information variable records candidate relationships for result resolution.
  • Corridor Refinement: The convex corridor generator removes repeatable polyhedrons so trajectory optimization does not reproduce unnecessary loops from a jerky teaching path.When the taught path discovers new space, a new polyhedron is appended to the corridor.
  • Corridor Refinement: The final corridor retains the teaching trajectory’s topology and contains no unnecessary loops.The corridor generation terminates when the teaching trajectory finishes.

V. SPATIAL-TEMPORAL GLOBAL TRAJECOTRY OPTIMIZATION

The system generates a flight corridor from the teaching path, then optimizes a piecewise Bézier trajectory within it. Spatial optimization minimizes squared jerk while representing the trajectory with Bernstein control points.

  • Spatial Trajectory Optimization: The flight corridor is generated as a sequence of convex polyhedrons that contains the teaching trajectory’s intended route.The corridor-generation procedure initializes a polyhedron, updates the drone pose, and adds or removes polyhedrons as the teaching path enters new or previously visited space.
  • Spatial Trajectory Optimization: An N-piece spatial trajectory is represented as a piecewise Bézier curve using Bernstein basis functions and control points.Each piece has its own control points and time duration, with the trajectory generated independently in the x, y, and z dimensions.
  • Spatial Trajectory Optimization: Squared jerk is minimized to reduce rotational motion and facilitate visual tracking.The objective is expressed as the quadratic form cT Qc, where c contains all control points and Q is a semidefinite Hessian matrix.
  • Spatial Trajectory Optimization: Higher-order derivatives of each Bézier segment are obtained from corresponding lower-order control points.This control-point representation supports the derivative calculations used in trajectory optimization.

1) Boundary Constraints:

The spatial trajectory is constrained at its endpoints, across segment junctions, and within the flight corridor. These constraints are combined with the spatial objective in a quadratic program, while kinodynamic feasibility is handled temporally.

  • Boundary Constraints: The trajectory satisfies initial and final position, velocity, and acceleration constraints through equality constraints on Bézier control points.Because a Bézier curve passes through its first and last control points, boundary conditions can be imposed directly on corresponding control points.
  • Continuity Constraints: Derivative continuity up to second order is enforced between consecutive trajectory pieces to ensure smoothness.Equality constraints link corresponding control points of adjacent Bézier curves.
  • Safety Constraints: The entire Bézier trajectory is kept inside its corresponding convex polyhedron by constraining its control points.The safety guarantee follows from the convex-hull property of Bézier curves and hyperplane constraints derived for each polyhedron.
  • Optimization Formulation: The spatial optimization is formulated as a quadratic program with affine equality and inequality constraints.Kinodynamic feasibility is not imposed through higher-order spatial constraints; it is handled by temporal optimization instead.

B. Temporal Trajectory Optimization

Temporal optimization re-times the spatial trajectory through a monotonically increasing mapping from original time t to optimized time τ. Its objective balances total flight time against motion aggressiveness.

  • Temporal Trajectory Optimization: Temporal optimization finds a re-timing function t(τ) that maps the original spatial-trajectory time to a new temporal profile.The mapping is piecewise over the spatial curve, and physical time requires t(τ) to be monotonically increasing.
  • Temporal Trajectory Optimization: Velocity and acceleration after re-timing are derived by substituting t(τ) into the spatial trajectory and applying the chain rule.The resulting velocity and acceleration are piecewise functions of the spatial derivatives and the derivatives of the time mapping.
  • Objective: The temporal objective minimizes total duration while penalizing changes in the time mapping to control motion aggressiveness.The regularization term trades off shorter flight time against control extremeness.
  • Objective: The weight ρ controls the aggressiveness–speed trade-off in the optimized motion.Larger ρ produces gentler motions, whereas ρ = 0 generates motions as fast as possible.
  • Continuity Constraints: Temporal trajectory continuity is enforced between consecutive pieces of the time mapping.The formulation also includes boundary constraints for the initial and final motion states.

2) Constraints:

The temporal formulation imposes endpoint, kinodynamic, discretization, and actuator-response constraints. After discretization and cone reformulation, it becomes a second-order cone program with guaranteed feasibility under the stated assumptions.

  • Constraints: Boundary constraints impose the initial and final velocity and acceleration of the temporal trajectory.These constraints ensure the optimized motion matches the required endpoint states.
  • Constraints: Kinodynamic constraints bound velocity and acceleration by the drone’s physical limits vmax and amax.These constraints are incorporated into the temporal optimization rather than the spatial quadratic program.
  • Discretization: The temporal optimization is discretized over each trajectory piece with resolution δt, producing discrete variables for acceleration-related functions.The discretization converts the continuous formulation into affine and conic constraints, with additional bounds on acceleration-rate changes.
  • Actuator Constraints: A bound δα limits the rate of acceleration change to account for actuator response time.The bound applies to acceleration changes in the original t domain rather than directly bounding jerk.
  • SOCP Reformulation: The temporal optimization is formulated as a standard Second Order Cone Program after cone and slack-variable reformulations.The reformulation uses quadratic cones and an affine objective representation.
  • Feasibility: Once a flight corridor is given, a spatial-temporal trajectory must exist under static initial and final states.The paper states that feasibility follows because time can be enlarged indefinitely and both spatial and temporal optimization problems have solutions.

VI. ONLINE LOCAL RE-PLANNING

The previous teach-and-repeat system could collide when maps changed or localization drifted without timely loop-closure correction. Online local mapping and re-planning address these conditions by detecting and avoiding obstacles locally.

  • The previous system assumed an intact environment and globally consistent pose estimation during trajectory execution.
  • Significant pose drift without timely loop-closure correction could make the actual global trajectory collide with mapped obstacles.
  • The local map uses onboard sensing and an ESDF to support collision-aware local trajectory adjustment.

A. Local Re-planning Framework

The local re-planning framework maintains a sliding local map and replans only within a temporal window of the global trajectory. It generates an obstacle-avoiding path that later rejoins the global route.

  • A local re-planning framework reactively wraps the global trajectory around unmodeled obstacles using a sliding onboard map.
  • FIESTA fuses depth data into a voxel-hashed occupancy map and incrementally updates its ESDF for local optimization.
  • Because sensing and computation are limited, re-planning operates within a temporal sliding window rather than globally.
  • When an obstacle blocks the windowed route, the system generates a local trajectory that avoids it and rejoins the global trajectory.
  • The local trajectory is initialized by fitting discretized horizon points to a uniform B-spline with equal knot-span durations.

2) B-spline Initialization:

Local B-spline optimization balances smoothness, obstacle clearance, and dynamic feasibility. An iterative post-process and time adjustment improve practical feasibility after unconstrained optimization.

  • The re-planned B-spline is optimized for smoothness, safety, and dynamical feasibility.
  • Smoothness is represented by a jerk-penalized cost that depends on the B-spline geometry rather than its time parametrization.
  • Safety and dynamic limits are imposed as soft costs evaluated at control points, using ESDF obstacle distances and velocity or acceleration violations.
  • The weighted total cost can be minimized with general optimization methods such as Gauss-Newton or Levenberg-Marquardt.
  • 4) Iterative Refinement:: After optimization, collision checks increase the collision weight when needed, while time spans are enlarged iteratively to remove velocity and acceleration infeasibilities.

A. Implementation Details

The system combines global mapping, localization, corridor generation, trajectory optimization, and local re-planning, and is evaluated in simulation and indoor flight experiments. Corridor and trajectory-generation tests report computational, spatial, and optimization advantages.

  • A. Implementation Details: All quadrotor modules, including localization, mapping, and planning, are released as ROS packages to support replication.
  • A. Implementation Details: The repeating system saves a globally anchored map and pose graph, then uses loop closure and pose-graph optimization to compensate for drift during flight.
  • B. Simulated Flight Test: Simulation uses randomly deployed obstacles, joystick-demonstrated teaching trajectories, a depth camera, and depth noise that triggers re-planning.
  • 1) Corridor Generation:: GPU acceleration improves corridor-generation speed 30 times at 0.075m resolution and 10 times at 0.25m resolution.
  • 1) Corridor Generation:: Polyhedrons capture significantly more corridor volume than axis-aligned cubes, while initialization causes only a slight volume sacrifice.
  • 2) Global Planning:: The proposed trajectory method outperforms the waypoint-based and gradient-based methods in length, time, and energy across simulated teach-and-repeat trials.
  • A. Implementation Details: Indoor experiments evaluate fast autonomous flights in a static obstacle environment with maximum velocity and acceleration set to 3m/s and 3m/s2.

D. Indoor Flight Test

The system is evaluated in cluttered indoor and outdoor settings, including aggressive racing and flights with unmapped or moving obstacles. Its planning pipeline combines corridor-based global trajectory generation with local perception and replanning to preserve safety while following the repeating trajectory.

  • Indoor Flight Test: A drone-racing experiment uses randomly deployed circles, arches, and tunnels, including a narrow 0.6 m-diameter circle relative to the drone’s 0.3 m × 0.3 m size.The experiment is designed to validate robustness in a complex, cluttered environment.
  • Indoor Flight Test: The system converts a virtually piloted teaching trajectory into a global repeating trajectory and tracks it under kinodynamic limits.The experiment uses a dense pre-built global map and sets the aggressiveness parameter to ρ = 0 for maximum speed within those limits.
  • Indoor Flight Test: Local replanning is tested against obstacles that are moved, added, or introduced dynamically during the repeating flight.This experiment evaluates the system’s response to significant changes in the drone-racing environment using a sliding local ESDF map.
  • Indoor Flight Test: Outdoor trials further test the system at higher aggressiveness in two natural scenes without GPS or other external positioning devices.The allowed velocity limits are 5 m/s and 7 m/s in the two trials, with 6 m/s2 acceleration limits.
  • Indoor Flight Test: The complete framework combines a GPU-accelerated flight corridor, iterative spatial-temporal optimization, local perception, and replanning for environmental changes, dynamic obstacles, and localization drift.The generated trajectory preserves the teaching trajectory’s topology, while online replanning maintains safety and respects the repeating reference.
Loading 1907.00520v1…