Research › Analysis

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

Gao, F. et al. · IEEE Trans. Robot. 36, 1526–1545 (2020) · Paper · Code

Q2 Computing analysis. All mathematical results and empirical findings are attributed to the original authors. We present our reading of the work as it relates to robotic automation.

What this paper solves

Human-piloted trajectories are geometrically valid but dynamically suboptimal. A pilot demonstrates a path through an environment once. The system refines that path into one the vehicle can fly aggressively and repeatably, then re-plans locally in real time when the environment changes. The three-phase name is the architecture: teach (human demonstration), repeat (optimized execution), replan (obstacle avoidance).

Key mathematical framework

Flight corridor construction

The free-space region around the taught path is approximated as a sequence of convex polyhedra. Each polyhedron is grown from a seed point on the reference trajectory using the Seed Convex Polyhedron Inflation (SCPI) algorithm. The corridor $C = {P_{1}, P_{2}, \dots, P_{n}}$ is the union of these overlapping polyhedra, and the optimized trajectory must satisfy:

T (t) \in C \forall t

Convex polyhedra give the optimizer significantly more geometric freedom than axis-aligned boxes, allowing it to find smoother, more energy-efficient paths through narrow or angled passages.

Spatial-temporal decoupling

Trajectory optimization is split into two sequential problems rather than solved jointly, making each sub-problem tractable in real time.

Spatial pass: Given corridor $C$ , find the arc-length-parameterized path $p (s)$ that minimizes total jerk subject to the containment constraint:

p (s) min \int \frac{d ^{3} p}{d s ^{3}}^{2} d s s.t. p (s) \in C

Temporal pass: Given the spatial path $p (s)$ , find a time parameterization $s (t)$ satisfying velocity, acceleration, and jerk limits derived from the vehicle's dynamic model. Solved as a convex optimization over the time allocation per path segment. Both passes iterate until convergence, typically in under 100ms on the onboard computer.

Local re-planning via gradient descent

When the onboard stereo camera detects an obstacle not in the global map, the system re-plans a local segment in a sliding window around the current position. The local re-planner minimizes:

J = λ_{s} \cdot J_{smooth} + λ_{c} \cdot J_{collision} + λ_{f} \cdot J_{feasibility}

where $J_{smooth}$ penalizes jerk, $J_{collision}$ penalizes proximity to obstacle voxels via signed distance field, and $J_{feasibility}$ enforces dynamic limits. Gradient descent on this combined cost produces a locally adjusted trajectory that avoids the new obstacle while remaining kinodynamically feasible. The sliding window ensures continuity with the global plan outside the replanned segment.

Localization: visual-inertial odometry with loop closure

State estimation uses a tightly-coupled VIO pipeline fusing IMU preintegration with stereo visual features. Drift is bounded over long trajectories by loop closure: when the vehicle revisits a previously mapped region, the system detects the match, computes the relative pose correction $Δ T \in S E (3)$ , and distributes it across the pose graph via g2o optimization, keeping the global map consistent even after extended flight.

Empirical observations

Flight speed: The system demonstrated reliable autonomous flight at up to 7 m/s through cluttered indoor environments, significantly above the speed at which purely reactive planners become unsafe
Re-planning latency: Local re-planning completed in under 20ms in tested scenarios, enabling real-time obstacle avoidance without interrupting global trajectory execution
Corridor generation: SCPI generated flight corridors for complex 3D paths in under 200ms on a ground station CPU, making the teach-to-execute transition practical for field deployment
Trajectory smoothness: Optimized trajectories consistently remained below actuator saturation limits (acceleration under 1.5g, jerk bounded), whereas raw human-taught trajectories frequently violated these bounds
Sim-to-real transfer: Policies trained and validated in simulation transferred to physical hardware without retuning, confirmed across multiple outdoor flight tests in varying wind conditions

What this means for robotic automation

The spatial-temporal decoupling is the transferable insight. Any automation task that involves a demonstrated path (a worker shows the robot what to do once) followed by repeated optimized execution is a candidate for this framework. The math does not care whether the vehicle is a quadrotor, a robotic arm, or a mobile ground platform. The corridor constraint and jerk-minimizing optimizer apply equally to any system with smooth dynamic limits.

The local re-planner is the robustness layer that makes this deployable in real environments. A system that can only repeat learned trajectories exactly is fragile. One that can re-plan locally around new obstacles in under 20ms can operate in environments that change between runs, which is the normal condition in any production setting.

The VIO localization backbone demonstrates that centimeter-accurate state estimation is achievable without GPS using only a camera and IMU. This is directly relevant to any indoor or GPS-denied automation deployment.