PaperlayerView exact revision on arXiv

Source-linked AI summary

Geometrically Constrained Trajectory Optimization for Multicopters

Zhepei Wang, Xin Zhou, Chao Xu, Fei Gao

arXiv:2103.00190v4cs.RO

TL;DR

Multicopter trajectory planning must handle geometric, dynamic, and task-specific constraints efficiently. The paper introduces MINCO-based parameterization, smooth constraint elimination, penalty functionals, and flatness-map differentiation to obtain unconstrained optimization with high-quality, fast solutions, while noting limitations from local optimization, spline relaxation, and penalties.

  • Problem

    Efficient multicopter planning must simultaneously address complex safety constraints, critical dynamic limits, and task-specific requirements.

  • Method

    The framework combines MINCO trajectory parameterization, smooth maps for geometric constraint elimination, decoupled constraint penalties, and backward differentiation of flatness maps.

  • Results

    The method achieves orders-of-magnitude faster computation while retaining high-quality solutions across benchmarks, simulations, and experiments.

  • Takeaways & Limitations

    The framework supports versatile multicopter flight planning with efficiency, generality, robustness, and constraint handling demonstrated across applications.

  • Takeaways & Limitations

    MINCO is a relaxation because optimal constrained solutions cannot generally be represented by polynomial splines, and the framework focuses on local solutions.

Abstract

from arXiv · show

We present an optimization-based framework for multicopter trajectory planning subject to geometrical configuration constraints and user-defined dynamic constraints. The basis of the framework is a novel trajectory representation built upon our novel optimality conditions for unconstrained control effort minimization. We design linear-complexity operations on this representation to conduct spatial-temporal deformation under various planning requirements. Smooth maps are utilized to exactly eliminate geometrical constraints in a lightweight fashion. A variety of state-input constraints are supported by the decoupling of dense constraint evaluation from sparse parameterization, and backward differentiation of flatness map. As a result, this framework transforms a generally constrained multicopter planning problem into an unconstrained optimization that can be solved reliably and efficiently. Our framework bridges the gaps among solution quality, planning efficiency, and constraint fidelity for a multicopter with limited resources and maneuvering capability. Its generality and robustness are both demonstrated by applications to different flight tasks. Extensive simulations and benchmarks are also conducted to show its capability of generating high-quality solutions while retaining the computation speed against other specialized methods by orders of magnitude. The source code of our framework is available at: https://github.com/ZJU-FAST-Lab/GCOPTER

I. INTRODUCTION

Multicopter trajectory planning must balance safety, dynamics, smoothness, task requirements, and real-time computation under limited resources. The framework addresses these challenges with a flexible trajectory class, constraint handling, and optimization designed for efficient, high-quality planning.

  • Multicopter planning must incorporate dynamics, smoothness, safety, and task requirements while operating under strict onboard size, weight, and power limits.
  • Existing approaches face challenges from complex safety data, nonlinear dynamics, fine discretization, and sparse trajectories’ limited temporal flexibility.
  • The framework introduces necessary and sufficient optimality conditions for multistage control-effort minimization, enabling direct solution construction with linear complexity.The conditions also provide information about problem-parameter sensitivity smoothness.
  • MINCO is a sparse trajectory class whose parameters directly control spatial and temporal profiles, with linear-complexity spatial-temporal deformation for user-defined objectives.Each MINCO element is an unconstrained control-effort minimizer by construction.
  • The framework combines smooth-map constraint elimination with constraint transcription to support geometrical and user-defined state-input constraints.Backward differentiation of flatness maps transfers constraint violations into gradients with respect to sparse parameters, while differentiable penalties enforce continuous-time constraints.
  • Benchmarks report orders-of-magnitude higher efficiency than existing methods while retaining comparable solution quality to general-purpose optimal-control solvers.Simulations and experiments further evaluate efficiency, optimality, robustness, and generality across multicopter flight tasks.
  • The framework combines capabilities from optimal control and motion planning to address large-scale multicopter planning with user-defined continuous-time state and control constraints.

B. Direct Optimization in Flat-Output Space

The framework optimizes multicopter trajectories in low-dimensional flat-output space, then maps them to full states and inputs. It models safety geometrically and supports user-defined continuous-time dynamic constraints under explicit regularity and environment assumptions.

  • Flat-output representation: Differential flatness represents multicopter states and inputs using finite derivatives of flat outputs, whose translation and yaw provide convenient planning variables.The flatness maps Ψx and Ψu recover state-input trajectories after optimizing z(t).
  • Problem formulation: The optimization minimizes quadratic control effort with time regularization while enforcing configuration constraints and user-defined state-input constraints.Configuration constraints are imposed through the obstacle-free region F, while dynamic constraints are expressed through equivalent constraints on flat-output derivatives.
  • Problem formulation: The trajectory problem is nontrivial because it combines continuous-time constraints with a nonconvex configuration-feasible set.The time regularization trades off control effort against total duration and can also strictly fix total time.
  • Configuration constraints: Geometrical feasibility is approximated by a union of closed convex primitives connected in a locally sequential manner.The framework considers closed balls or bounded convex polytopes, with endpoint translations inscribed in the first and last primitives.
  • Design objective: The framework targets a computationally efficient solver that retains flexibility for different task-specific constraints.The stated objective is to construct an efficient solver for the trajectory optimization while preserving support for GD.

IV. MULTI-STAGE CONTROL EFFORT MINIMIZATION

The paper derives necessary and sufficient optimality conditions for multi-stage control-effort minimization with spatial and temporal parameters. These conditions characterize unique piecewise-polynomial trajectories and their smoothness across stage boundaries.

  • Problem setup: The multi-stage problem uses fixed timestamps, boundary conditions, and intermediate derivative conditions to define each polynomial stage.At an intermediate timestamp, di specifies how many derivatives of z(t) are fixed.
  • Optimality conditions: Necessary and sufficient optimality conditions cover all settings of intermediate derivative constraints, boundary data, and timestamps, guaranteeing a unique optimal trajectory.The result directly constructs trajectories from spatial-temporal parameters.
  • Derivation: The derivation applies the Hybrid Maximum Principle to a Mayer-form augmented system with stagewise state switching.The augmented state resets its accumulated-cost component at stage boundaries while preserving the relevant state continuity.
  • Smoothness: The optimal control is s −di −1 times continuously differentiable at each intermediate timestamp, while the optimal state is 2s −di −1 times continuously differentiable.For multi-stage jerk minimization, state continuity is required through acceleration, while higher derivatives can remain continuous as part of the optimum.
  • Optimality conditions: Each stage is parameterized by a polynomial of degree 2s −1, with boundary and intermediate conditions linked by continuity requirements.The theorem states polynomial parameterization, boundary conditions, intermediate conditions, and cross-stage smoothness as the optimality characterization.

C. Minimization Without Cost Functional

The optimal trajectory coefficients are obtained by solving a banded linear system rather than evaluating the cost functional or its gradient. This yields linear time and space complexity.

  • Direct construction: The unique optimal trajectory can be constructed directly from its optimality conditions without explicit or implicit evaluation of the cost functional.The construction has linear complexity in both time and space.
  • Trajectory representation: An M-piece trajectory is represented by polynomial coefficients c and a positive stage-duration vector T.Each piece uses a degree N = 2s −1 polynomial basis, with timestamps formed by cumulative stage durations.
  • Linear system: Enforcing the optimality conditions produces a banded linear system for the coefficient matrix c.The system is assembled from boundary and intermediate derivative constraints through block matrices Ei and Fi.
  • Complexity: O(M) time and space complexity follows from solving the nonsingular banded system with Banded PLU Factorization.Uniqueness ensures the system matrix is nonsingular for any positive time vector T.

D. MINCO Trajectories With Spatial-Temporal Deformation

TMINCO trajectories use intermediate points and stage durations as sparse spatial-temporal parameters for deformation. Their gradients can be propagated in linear complexity using the banded factorization.

  • Motivation: Spatial-temporal deformation addresses task-specific requirements that may require changing both a trajectory’s geometry and timing.Examples include perception quality in active SLAM and occlusion rate in aerial videography.
  • TMINCO representation: TMINCO compactly parameterizes control-effort-minimizing trajectories using intermediate points q and a positive time vector T.The optimality conditions map any pair of q and T to a trajectory in the MINCO class.
  • Objective deformation: A user-defined C2 objective K(c, T) is optimized on TMINCO through the induced function W(q, T) and its gradients.The method propagates derivatives from ∂K/∂c and ∂K/∂T to the sparse parameters q and T.
  • Gradient propagation: The existing banded factorization is reused to compute gradient information efficiently, avoiding inversion of the transpose system matrix.The matrix G can be computed once to obtain derivatives with respect to all intermediate points.

V. GEOMETRICALLY CONSTRAINED FLIGHT TRAJECTORY OPTIMIZATION

The framework relaxes multicopter trajectory optimization into TMINCO, using spatial-temporal deformation, exact geometrical constraint elimination, and a time-integral penalty for continuous-time constraints.

  • TMINCO unifies flight trajectory optimization with time regularization, spatial constraints, and continuous-time constraints.It enables spatial-temporal deformation while preserving sparse trajectory parameterization.

A. Temporal Constraint Elimination

The framework converts temporal-domain constraints into unconstrained optimization variables through explicit diffeomorphisms. These maps preserve relevant local optimality conditions while enabling efficient gradient-based optimization.

  • Constraint elimination: Explicit diffeomorphisms map constrained duration variables into Euclidean surrogate variables, so optimization can proceed without repeated retractions.The transformed variables automatically satisfy the original temporal-domain constraints.
  • Computational structure: Cost and gradient evaluation through the MINCO parameterization can be performed with O(M) complexity.The result follows from propagating derivatives through the polynomial-spline coefficients determined by q and T.
  • Temporal domain: Temporal durations lie in constrained positive or simplex-like domains, where the cost may become unbounded near domain boundaries.The feasible domain for fixed-total-time trajectories is the relative interior of a simplex, while positive durations require T_i > 0.
  • Constraint elimination: The duration transformation supports unconstrained optimization over τ while allowing initial feasible duration vectors to be mapped back through an inverse transformation.For positive durations, the exponential map T = e[τ] enforces positivity; for fixed-total-time durations, τ_i = ln(T_i/T_M) provides an inverse map.
  • Optimality preservation: The transformed cost remains nonconvex, but the diffeomorphism preserves first- and second-order optimality conditions without introducing spurious or removing existing local minima.This property applies to the stated C2 transformations and their corresponding transformed objectives.

B. Spherical Spatial Constraint Elimination

The framework eliminates spherical and ball-shaped spatial constraints by mapping unconstrained Euclidean variables onto the feasible set. The resulting smooth surjection preserves desired local minima while permitting direct unconstrained optimization.

  • Ball constraints: A composition of inverse stereographic and orthographic projections maps R^n smoothly onto a closed ball, making feasibility automatic during optimization.The Euclidean variable ξ remains unconstrained while the transformed point q stays within the ball.
  • Optimality preservation: A convex function’s constrained minimum in a 2-D ball is transformed into a nonconvex unconstrained objective while preserving the corresponding local minimum.The figure states that no additional local minimum is introduced by the transformation.
  • Ball constraints: The spherical construction introduces a new Euclidean coordinate ξ whose optimization implicitly satisfies the spatial ball constraint.Gradients with respect to ξ are obtained by differentiating the mapping layer and propagating gradients from q.
  • Optimality preservation: Although the ball map is not one-to-one, its component structure preserves first-order local-minimum conditions across the transformed spaces.The paper therefore concludes that the map does not create extra spurious local minima or remove existing ones.

C. Polyhedral Spatial Constraint Elimination

Polyhedral constraints are converted into simplex and ball constraints, then eliminated through smooth variable transformations. Continuous-time constraints are handled separately with differentiable integral penalties evaluated independently of sparse trajectory parameterization.

  • Polytope elimination: The simplex is transformed using entry-wise squaring and a ball map, producing a smooth surjection from unconstrained Euclidean variables onto the polytope.Every transformed variable remains feasible, and the polytope boundary remains attainable.
  • Polytope representation: A convex polytope is represented in barycentric coordinates and converted into a standard simplex through a linear transformation.This conversion increases decision-variable dimension but introduces no additional nonlinearity at that stage.
  • Optimality preservation: The added nonlinearity from the square-map conversion is supported as preserving first- and second-order optimality conditions for inequality-constrained problems.The paper reports that the transformation does not exclude the desired minimum or practically produce undesired minima.
  • Continuous-time constraints: After direct spatial constraints are eliminated, trajectory deformation can be optimized over unconstrained variables while satisfying continuous-time constraints through penalty terms.The framework decouples dense constraint evaluation from the sparse parameterization of the trajectory.
  • Penalty design: Penalty methods avoid requiring a feasible initial guess, although numerical quadrature makes constraint approximation inevitable.The framework uses penalties because exact continuous-time integration is unavailable and feasible initial guesses may be difficult to construct.
  • Continuous-time constraints: The integral penalty grows rapidly when trajectory constraints are violated and remains zero when sampled constraints are satisfied.Quadrature approximates the integral with adjustable resolution, using a trapezoidal rule whose values and gradients can be computed in parallel.

E. Trajectory Optimization via Unconstrained NLP

The framework parameterizes trajectories sparsely, transforms geometrical and dynamic constraints, and solves the resulting relaxed problem as an unconstrained nonlinear program. Benchmarks report orders-of-magnitude speedups while retaining trajectory quality and linear complexity.

  • Unconstrained NLP: Spatial-temporal deformation of TMINCO is optimized through a lightweight unconstrained nonlinear program.The objective combines time-regularized control effort with a penalty functional for task-specific requirements.
  • Unconstrained NLP: Flat-output trajectories are parameterized as TMINCO, direct constraints are eliminated by transformations, and state-input constraints are mapped through flatness maps.Gradients are propagated through the resulting layers and the relaxation is solved using L-BFGS.
  • Large-scale minimization: The proposed scheme improves computation speed by orders of magnitude against benchmarked jerk- and snap-minimization methods while retaining O(M) complexity.The comparison covers problem scales from middle-sized to very large piece counts.
  • Safe-flight corridors: In randomly generated safe-flight corridors, the proposed method produces smooth trajectories whose speed persistently reaches the maximum, including narrow and twisted corridors.Both polyhedron-shaped and ball-shaped corridors are handled.
  • Benchmarks: Against specialized methods, the proposed method retains comparable solution quality while achieving higher efficiency and optimizing time allocation.Methods using integer interval allocation have lower success rates, while heuristic allocation can increase control effort.

C. SE(3) Motion Planning in Quotient Space

The framework plans SE(3) motion by optimizing translation in a quotient space and recovering rotation and inputs through flatness maps. It handles narrow-gap constraints while maintaining dynamic-limit fidelity and high computation speed.

  • Quotient-space formulation: Quotient-space planning optimizes translational trajectories while recovering full SE(3) states and inputs through flatness maps.This decomposes rotational safety reasoning from translational trajectory generation.
  • Constraints: Safety requires the multicopter ellipsoid to remain inside the free region throughout the motion, alongside velocity, thrust, and body-rate limits.The free region is represented using obstacle-free convex primitives that can contain a feasible vehicle ellipsoid.
  • Narrow-gap planning: As gaps narrow, larger angular rates and higher thrust are required, but the method persistently enforces these input limits.The narrow-gap simulation uses explicit limits on speed, thrust, and body rate.
  • Narrow-gap planning: The planner finds all feasible narrow-gap motions and supports SE(3) planning at a frequency of at least 100 Hz.The penalty functional maintains continuous-time tightness of the minimum-thrust constraint in the tested gap settings.
  • Real-world validation: Real-world experiments include consecutive windows with roll angles from 30° to 90° and approximately 20.0 seconds of safe back-and-forth flight.The experiments use restricted free volumes and limited vehicle maneuverability.

VII. DISCUSSION AND CONCLUSION

The framework is presented as flexible, extensible, and efficient across multicopter planning applications. The authors also identify local-minimum, representation, and penalty-functional limitations.

  • Extensions: The framework can adopt more accurate vehicle dynamics, support time-dependent constraints, and exploit parallelism in timestamp-wise computations.The authors identify rotor drag models and moving-obstacle constraints as supported extensions.
  • Extensions: The framework has been extended to swarm navigation in unknown environments.Formulation details and real-world flights are reported in a technical report.
  • Limitations: Optimization-based planning focuses on local solutions and can suffer from shallow local minima.The authors suggest interleaving sampling- or graph-search-based strategies as an alleviation.
  • Limitations: MINCO is a relaxation because optimal trajectories with geometrical constraints cannot generally be represented by polynomial splines.The reported results nevertheless show high-quality solutions with much faster computation.
  • Limitations: Zero constraint violation requires unbounded smoothing or penalty weights and unbounded quadrature resolution, although small violations are empirically acceptable for multicopter navigation.This limitation arises from the penalty functional.

APPENDIX

The appendix constructs a spline basis for multistage control-effort minimization and establishes nonsingularity of its coefficient matrix. This yields existence, uniqueness, and sufficiency of the optimal solution.

  • Spline construction: The considered M-piece polynomial 2s-order spline space is represented as a linear space with an explicit basis.The basis is constructed from an extended partition of the original timestamps.
  • Spline construction: Specified derivative orders and timestamps are interpolated in the basis without requiring the derivative values during basis construction.The timestamps are defined from the original trajectory partition.
  • Nonsingularity: The coefficient matrix B is shown to be nonsingular for the original problem settings.The proof verifies the required interval condition for all relevant indices.
  • Optimality: Nonsingularity implies existence and uniqueness of the solution, while the optimality conditions guarantee one unique solution in each decoupled dimension.The latter establishes sufficiency of the conditions.
  • Variable transformation: A nonsingular Jacobian preserves stationary-point correspondence and congruence of the Hessians under the variable transformation.This supports equivalence of local curvature properties between the original and transformed formulations.
Loading 2103.00190v4…