Geometrically Constrained Trajectory Optimization for Multicopters

• Multicopter planning must incorporate dynamics, smoothness, safety, and task requirements while operating under strict onboard size, weight, and power limits.Source: §I · p. 1
• Existing approaches face challenges from complex safety data, nonlinear dynamics, fine discretization, and sparse trajectories’ limited temporal flexibility.Source: §I · p. 1
• 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.Source: §I · p. 1
• 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.Source: §I · p. 2
• 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.Source: §I · p. 2
• 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.Source: §I · p. 2
• 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.Source: §II · p. 2, B. Sampling-Based Motion Planning · pp. 2–3

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).Source: B. Direct Optimization in Flat-Output… · p. 4

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.Source: B. Direct Optimization in Flat-Output… · p. 4
• 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.Source: C. Problem Formulation · p. 4

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.Source: C. Problem Formulation · p. 4

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.Source: C. Problem Formulation · p. 4

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.Source: A. Unconstrained Control Effort… · p. 5

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.Source: B. Optimality Conditions · pp. 5, 7

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.Source: B. Optimality Conditions · p. 5

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.Source: B. Optimality Conditions · pp. 6–7

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.Source: B. Optimality Conditions · p. 7

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.Source: C. Minimization Without Cost Functional · p. 7

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.Source: C. Minimization Without Cost Functional · p. 7

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.Source: C. Minimization Without Cost Functional · p. 7

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.Source: C. Minimization Without Cost Functional · p. 7

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.Source: D. MINCO Trajectories With… · p. 7

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.Source: D. MINCO Trajectories With… · pp. 7–8

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.Source: D. MINCO Trajectories With… · p. 8

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.Source: D. MINCO Trajectories With… · p. 8

• TMINCO unifies flight trajectory optimization with time regularization, spatial constraints, and continuous-time constraints.It enables spatial-temporal deformation while preserving sparse trajectory parameterization.Source: §V · p. 9

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.Source: A. Temporal Constraint Elimination · p. 9

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.Source: A. Temporal Constraint Elimination · p. 9

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.Source: A. Temporal Constraint Elimination · p. 9, Figure 3 · p. 9

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.Source: A. Temporal Constraint Elimination · p. 9

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.Source: A. Temporal Constraint Elimination · p. 9

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.Source: B. Spherical Spatial Constraint… · p. 10, Figure 4 · p. 10

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.Source: Figure 5 · p. 10

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.Source: B. Spherical Spatial Constraint… · p. 10

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.Source: B. Spherical Spatial Constraint… · p. 10

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.Source: Figure 6 · p. 11, C. Polyhedral Spatial Constraint… · p. 11

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.Source: C. Polyhedral Spatial Constraint… · p. 11

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.Source: C. Polyhedral Spatial Constraint… · p. 11

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.Source: C. Polyhedral Spatial Constraint… · p. 11, D. Time Integral Penalty Functional · p. 11

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.Source: D. Time Integral Penalty Functional · pp. 11–12

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.Source: D. Time Integral Penalty Functional · p. 12

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.Source: E. Trajectory Optimization via… · p. 12
• 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.Source: E. Trajectory Optimization via… · p. 12, B. Trajectory Generation Within Safe… · p. 13

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.Source: A. Large-Scale Unconstrained Control… · p. 12

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.Source: Figure 9 · p. 13, B. Trajectory Generation Within Safe… · p. 13

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.Source: B. Trajectory Generation Within Safe… · pp. 14–15

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.Source: C. SE(3) Motion Planning in Quotient… · p. 15

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.Source: C. SE(3) Motion Planning in Quotient… · p. 15

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.Source: Figure 15 · p. 16, C. SE(3) Motion Planning in Quotient… · p. 16
• 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.Source: C. SE(3) Motion Planning in Quotient… · p. 16

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.Source: C. SE(3) Motion Planning in Quotient… · p. 17

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.Source: A. Extensions · p. 17
• The framework has been extended to swarm navigation in unknown environments.Formulation details and real-world flights are reported in a technical report.Source: A. Extensions · p. 17

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.Source: B. Limitations · p. 17
• 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.Source: B. Limitations · p. 17
• 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.Source: B. Limitations · pp. 17–18

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.Source: A. Proof of Sufficiency in Theorem 2 · p. 18
• 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.Source: A. Proof of Sufficiency in Theorem 2 · p. 18

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.Source: A. Proof of Sufficiency in Theorem 2 · p. 18

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.Source: A. Proof of Sufficiency in Theorem 2 · p. 18

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.Source: B. Proof of Proposition 2 · p. 19