PaperlayerView exact revision on arXiv

Source-linked AI summary

Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight

Boyu Zhou, Fei Gao, Luqi Wang, Chuhao Liu, Shaojie Shen

arXiv:1907.01531v2cs.RO

TL;DR

Fast autonomous quadrotor flight requires safe and dynamically feasible trajectories generated efficiently in complex environments. The paper combines kinodynamic search, B-spline optimization, and iterative time adjustment, and reports effective performance in complex simulations and challenging real-world tasks.

  • Problem

    Existing methods do not reliably generate safe, kinodynamically feasible trajectories quickly under limited onboard resources, and often enforce velocity and acceleration constraints conservatively.

  • Method

    The method searches discretized control space for a safe minimum-time initial path, refines it with convex-hull-based B-spline optimization using EDF gradients and dynamic constraints, then adjusts non-uniform B-spline timing.

  • Results

    The method generates high-quality trajectories in cluttered environments in shorter time with higher success rate, and is demonstrated in complex simulations and challenging real-world fast-flight tasks.

  • Takeaways & Limitations

    The system supports aggressive, dynamically feasible motion and repeated replanning for challenging fast autonomous flight.

Abstract

from arXiv · show

In this paper, we propose a robust and efficient quadrotor motion planning system for fast flight in 3-D complex environments. We adopt a kinodynamic path searching method to find a safe, kinodynamic feasible and minimum-time initial trajectory in the discretized control space. We improve the smoothness and clearance of the trajectory by a B-spline optimization, which incorporates gradient information from a Euclidean distance field (EDF) and dynamic constraints efficiently utilizing the convex hull property of B-spline. Finally, by representing the final trajectory as a non-uniform B-spline, an iterative time adjustment method is adopted to guarantee dynamically feasible and non-conservative trajectories. We validate our proposed method in various complex simulational environments. The competence of the method is also validated in challenging real-world tasks. We release our code as an open-source package.

I. INTRODUCTION

The paper targets robust, efficient quadrotor trajectory generation for fast autonomous flight, addressing limited planning time, onboard computation, and conservative dynamic constraints. It combines kinodynamic search, B-spline optimization, and time adjustment, with simulation and real-world validation.

  • Motivation: High-speed flight in unknown environments requires safe, kinodynamically feasible trajectories to be regenerated rapidly under limited onboard resources.The paper identifies planning efficiency and robustness as essential for avoiding emergent threats.
  • Evaluation: The method aims to generate high-quality trajectories in cluttered environments faster and with a higher success rate than existing works.The authors also report aggressive motion under dynamic-feasibility constraints and validation in simulation and challenging real-world fast-flight tasks.
  • Approach: The proposed system combines kinodynamic path searching, B-spline optimization, and iterative time adjustment into one online planning method.The components respectively address initial path generation, smoothness and clearance, and dynamically feasible time allocation.
  • Approach: The B-spline optimization uses convex-hull properties to incorporate gradient information and dynamic constraints efficiently.This formulation is designed to improve the initial path while converging quickly to smooth, safe, dynamically feasible trajectories.
  • Evaluation: The implementation is released as an open-source ROS package.The paper presents extensive simulation and real-world evaluation alongside the released source code.

1) Hard-constrained methods:

Prior trajectory-generation methods use hard or soft constraints and polynomial or nonlinear optimization, while this paper uses kinodynamic search with dynamically valid motion primitives. The search expands, prunes, and checks primitives in a voxel grid, with analytic expansion providing an additional goal-connection strategy.

  • Hard-constrained methods: Minimum-snap and related hard-constrained methods generate piecewise-polynomial trajectories through quadratic programming and waypoint-based safety enforcement.Other approaches use sequences of geometric free-space regions or continuous-time polynomial optimization.
  • Kinodynamic path searching: Figure 2 distinguishes ordinary motion primitives from analytic expansion, showing red primitive curves and a purple analytic-expansion curve.The analytic expansion is the goal-connection mechanism described for the search.
  • Soft-constrained methods: Soft-constrained methods use gradient information to improve safety but can suffer from local minima and weak guarantees of success rate and kinodynamic feasibility.The proposed optimization also uses gradients while redesigning the formulation to avoid computationally expensive line integrals.
  • Kinodynamic path searching: Kinodynamic search uses motion primitives that respect quadrotor dynamics as graph edges instead of straight-line edges.The front end is derived from hybrid-state A* and searches for safe, dynamically feasible paths with minimum time duration and control cost.
  • Kinodynamic path searching: The search iteratively expands the voxel map, prunes primitives ending in the same voxel, and checks the remaining primitives for safety and dynamic feasibility.The loop terminates when a primitive reaches the goal or analytic expansion succeeds.
  • Motion primitives: Motion primitives are generated by applying discretized control inputs for a fixed duration to a differential-flatness-based polynomial model.For the practical double-integrator choice, each control axis is uniformly discretized, producing (2r + 1)^3 primitives.

B. Actual Cost and Heuristic Cost

The search scores trajectories using accumulated primitive costs and an admissible heuristic based on a closed-form minimum-cost connection to the goal. The resulting evaluation cost combines the actual path cost with this heuristic.

  • Cost formulation: A trajectory cost is defined to optimize time duration and control cost.The primitive edge cost is ec = (∥ud∥2 + ρ)τ for discretized input ud applied over duration τ.
  • Heuristic cost: The heuristic computes a closed-form trajectory from the current state to the goal using Pontryagin’s minimum principle.The optimal feasible root of the derivative of the closed-form cost with respect to time determines Th, whose cost becomes hc.
  • Combined cost: The search priority cost is fc = gc + hc = gc + J*(Th).This combines accumulated path cost with the minimum feasible heuristic connection cost.

C. Analytic Expansion

Analytic expansion compensates for discretized controls that rarely terminate exactly at the goal by directly computing a candidate connection from the current node to the goal. A safety and dynamic-feasibility check determines whether this connection can terminate the search early.

  • C. Analytic Expansion: Discretized control inputs make exact goal-state termination difficult, motivating analytic expansion from a popped node.The candidate trajectory is computed using the same closed-form approach as the heuristic.
  • C. Analytic Expansion: The search terminates early when the analytic connection passes safety and dynamic-feasibility checks.The strategy is especially effective in sparse environments because it can terminate searching earlier with a higher success rate.
  • D. Optimality and Completeness: Path searching has no theoretical guarantee of optimality or completeness, although evaluation reports satisfactory practical performance.The method can be extended with variable-duration primitives and variable-resolution voxel grids to provide stronger completeness guarantees.

IV. B-SPLINE TRAJECTORY OPTIMIZATION

The method refines an initial path with B-spline optimization to improve smoothness and clearance while enforcing safety and dynamic feasibility through convex-hull-based constraints.

  • B-spline optimization improves the initial path’s smoothness and clearance using Euclidean distance field gradients and dynamic constraints.The optimization converges quickly to produce smooth, safe, and dynamically feasible trajectories.
  • A. Uniform B-splines: A B-spline is a piecewise polynomial defined by its degree, control points, and knot vector, with uniform splines using identical knot spans.The trajectory is parameterized over a time interval and evaluated span by span.
  • A. Uniform B-splines: The derivative of a B-spline is also a B-spline, allowing velocity and acceleration properties to be handled through derivative control points.Derivative control points are computed from the original control points and knot span.
  • B. Convex Hull Property: The convex hull property supports trajectory-wide safety and dynamic-feasibility guarantees by constraining control points and checking hull clearance.Each spline segment lies within the convex hull of its corresponding control points.
  • B. Convex Hull Property: Constraining velocity and acceleration control points within [−vmax, vmax]3 and [−amax, amax]3 suffices for dynamic feasibility.These derivative control points are calculated using the knot span.
  • B. Convex Hull Property: Ensuring the relevant control-point distance condition makes every B-spline convex hull collision-free.The guarantee follows from bounding the distance between an occupied voxel and any point inside the hull.

C. Problem Formulation

The formulation optimizes selected B-spline control points with smoothness, collision, and dynamic-feasibility costs, then adjusts timing when obstacle avoidance makes motion overly aggressive.

  • Only the interior control points are optimized, while the first and last pb points remain fixed because they determine the boundary state.For N + 1 control points, the optimized subset contains N + 1 − 2pb points.
  • The total cost combines smoothness, collision, and soft velocity and acceleration penalties, weighted by λ1, λ2, and λ3.These weights trade off smoothness, safety, and dynamic feasibility.
  • The smoothness term uses an elastic-band cost based on geometric control-point relationships rather than time-dependent squared snap or jerk integrals.This choice remains meaningful when time allocation is adjusted after optimization.
  • The collision cost acts as a repulsive force on control points according to their distance from the nearest obstacle.A differentiable potential cost uses dthr as the obstacle-clearance threshold.
  • Velocity and acceleration violations are penalized when they exceed the maximum allowable values vmax and amax.The one-dimensional velocity penalty is defined using the same penalty structure as the collision cost.
  • Gradient-based obstacle avoidance can lengthen trajectories and produce infeasible motion when the original trajectory is already near physical limits.The resulting need to cover more distance in the same time can cause over-aggressive motion.
  • Time adjustment changes non-uniform B-spline knot spans to restore dynamic feasibility without imposing over-conservative constraints.The adjustment uses relations between derivative control points and time allocation.

A. Non-uniform B-spline

Non-uniform B-splines permit independently sized knot spans, whose adjustment can enforce derivative-based dynamic feasibility. The method changes spans associated with infeasible velocity or acceleration control points.

  • A. Non-uniform B-spline: Non-uniform B-splines differ from uniform B-splines because each knot span can be independently sized.Their derivative control points depend on the knot-span durations.
  • A. Non-uniform B-spline: The convex hull property makes dynamic feasibility sufficient to enforce on all first- and second-order derivative control points.Keeping these control points inside the feasible domain ensures the represented trajectory is dynamically feasible.
  • A. Non-uniform B-spline: An infeasible velocity component can be corrected by scaling its associated knot-span duration according to the velocity limit.The resulting component reaches vmax and lies within [−vmax, vmax].
  • A. Non-uniform B-spline: Acceleration feasibility is enforced similarly by scaling the coupled sequence of knot spans influencing the acceleration control point.The adjustment yields an acceleration component bounded by amax within [−amax, amax].

C. Iterative Time Adjustment

The iterative time-adjustment algorithm enforces dynamic feasibility by repeatedly locating infeasible derivative control points and modifying their corresponding knot spans. Slightly conservative scaling factors limit excessive time-span extension.

  • C. Iterative Time Adjustment: Algorithm 2 iteratively identifies infeasible velocity and acceleration control points and adjusts their corresponding knot spans.The procedure applies separate adjustment operations to velocity and acceleration control points.
  • C. Iterative Time Adjustment: Because knot spans and derivative control points influence one another, the algorithm repeatedly updates affected spans until feasibility is enforced.Each knot span influences several control points, and each control point depends on several spans.
  • C. Iterative Time Adjustment: Constants αv and αa slightly larger than 1 prevent any time span from being extended excessively.These bounds are applied during the velocity and acceleration adjustments.

VI. IMPLEMENTATION DETAILS

The implementation uses fixed planning, optimization, and time-adjustment parameters and evaluates the system in two real-world flight settings. The experiments use different quadrotor platforms and sensing configurations.

  • VI. IMPLEMENTATION DETAILS: The system is implemented in C++11 with NLopt, using fixed parameters across all experiments.The reported settings include r = 2, τ = 0.5, λ1 = 10.0, λ2 = 0.8, λ3 = 0.01, and αa = αv = 1.1.
  • VI. IMPLEMENTATION DETAILS: Fast autonomous flight in unknown cluttered environments uses a self-developed quadrotor equipped with a Velodyne VLP-16 3-D LiDAR.LOAM, an EKF, mapping, planning, and control support the experiment.
  • VI. IMPLEMENTATION DETAILS: Aggressive-flight replanning uses a lighter, more agile quadrotor with OptiTrack pose feedback and a pre-built environment map.Planning and control run onboard an Nvidia TX2 computer.

B. Re-planning Strategy

The planner uses receding-horizon replanning within the known sensing range, triggering updates after newly detected collisions or at fixed intervals. Incremental EDF updates support efficient optimization, while path-searching experiments expose speed–quality trade-offs.

  • B. Re-planning Strategy: Receding-horizon planning generates trajectories only within the currently known space to avoid unnecessary searches beyond the sensing range.Path searching stops when a motion primitive exits the known range, followed by optimization and time adjustment.
  • B. Re-planning Strategy: Replanning is triggered either by a collision with a newly emergent obstacle or periodically at fixed time intervals.Periodic updates incorporate the most recent environmental information.
  • C. Euclidean Distance Field: The Euclidean distance field is computed in O(n) time and updated incrementally only within the sensing range.Trilinear interpolation improves distance and gradient accuracy while avoiding costly global updates.
  • 1) Comparison of Path Searching:: Compared with method, the proposed path search is faster by one order of magnitude and tends to produce shorter-duration paths.Both methods generate kinodynamically feasible trajectories, while the proposed method has slightly higher control cost.
  • 1) Comparison of Path Searching:: Coarser voxel grids increase search efficiency but raise control cost and lower success rate.The trade-off arises because pruning reduces search complexity while discarding feasible and potentially superior paths.

2) Comparison of Optimization:

The proposed optimization converges faster than the previous gradient-based method while producing smoother trajectories. Experiments further evaluate autonomous and aggressive flight in challenging environments.

  • Trajectory smoothness: Despite receiving less optimization time, the proposed method produces smoother trajectories than the previous method.Smoothness is measured by the integral of squared jerk.
  • Optimization convergence: The proposed optimization cost decreases to zero within 3ms, whereas the previous method takes significantly longer under the same 30ms limit.Both cost profiles are normalized to [0,1] for comparison.
  • Autonomous flight: Fully autonomous flights use only a 5m-radius local map in an unknown, unstructured environment with limited perception range.The restricted map is smaller than the quadrotor’s real perception range.
  • Aggressive flight: Aggressive-flight experiments repeatedly replan and immediately execute trajectories after a human changes the goal arbitrarily.The configured maximum velocity and acceleration are 2.5m/s and 1.5m/s2, respectively.
  • Evaluation and scope: The method is validated in complex environments and challenging real-world tasks, while future work targets large-scale or dynamic environments.The authors also plan to extend trajectory optimization to swarm problems.
Loading 1907.01531v2…