PaperlayerView exact revision on arXiv

Source-linked AI summary

NMPP: Nonlinear Model Predictive Planning for Agile UAV Flight in Cluttered Environments

Ondřej Procházka, Juraj Pauko, Robert Pěnička, Martin Saska

arXiv:2610.08695v1cs.RO

TL;DR

Fast UAV flight in clutter requires online trajectories that respect both obstacles and vehicle limits, but prior approaches may constrain motion to corridors or trade avoidance against tracking. NMPP plans a full-state reference with hard obstacle constraints; it reports lower tracking error than two planners and successful flights in simulation and outdoors.

  • Problem

    Online UAV planning in initially unknown clutter must produce trajectories that are collision-free and dynamically feasible, while existing corridor and soft-cost approaches limit agility or trade avoidance against tracking.

    Source: §I · p. 1

  • Method

    NMPP uses NMPC to generate a full-state reference for a separate controller, imposing LiDAR-perceived obstacles as hard constraints on full quadrotor dynamics.

    Source: §I · pp. 1–2

  • Results

    Position RMSE was 58–67% lower than the linear MPC planner and 41–70% lower than the polynomial planner; NMPP succeeded in 86% of agile-profile flights versus SUPER’s 26%.

    Source: §VI · p. 8

  • Takeaways & Limitations

    The planner was deployed on a real UAV, which avoided obstacles at speeds up to 5.5 m s−1 in gusty wind while running everything onboard.

    Source: §VI · p. 8

Abstract

from arXiv · show

Flying a quadrotor through a cluttered environment requires not only planning a collision-free reference trajectory based on perceived obstacles, but the reference also needs to be dynamically feasible and within the actuation limits of the vehicle, so that the controller can track it precisely. Existing methods either optimize a smooth polynomial inside a convex corridor, which limits agility, or treat obstacles as soft costs traded against tracking performance. We propose a Nonlinear Model Predictive Planning (NMPP) that imposes perceived obstacles as hard geometric constraints and hands a full-state reference to an obstacle-blind SE(3) controller. Our planner achieves a 58-67 % lower position RMSE than a linear Model Predictive Control trajectory planner and a 41-70 % lower RMSE than a polynomial trajectory planner. It also completes all forest flights with up to 9.5 m/s speed without collisions, and achieves 86 % flight success rate under a more aggressive speed profile where a state-of-the-art planner has only 26 % success rate. The real-world deployment showed reliable execution flying up to 5.5 m/s in an unknown cluttered environment.

SUPPLEMENTARY MATERIAL

NMPP combines collision avoidance and dynamic feasibility in one onboard trajectory-planning optimization. It addresses limits of corridor-based polynomial planning and soft-cost obstacle avoidance, and its reported results span simulation and real-world flight.

I. INTRODUCTION

  • Fast flight through perceived obstacles requires online trajectories that are both collision-free and consistent with UAV dynamics and actuation limits.The passages identify endurance-limited coverage as a motivation and note that separately planning paths and trajectories can leave paths dynamics-unaware and trajectories obstacle-unaware.Source: §I · p. 1
  • The planning problem is nonconvex, must accommodate a variable obstacle count at fixed computational size, and must guard against collisions between discrete prediction nodes.Corridor-based polynomial methods avoid nonconvexity by restricting motion to convex free space, but smooth polynomials limit use of dynamic limits; control-loop approaches use soft obstacle costs that trade avoidance against tracking.Source: §I · p. 1
  • NMPP uses NMPC to plan a full-state reference for a separate SE(3) controller, imposing LiDAR-perceived obstacles as hard constraints on full quadrotor dynamics.A fixed number of ellipsoids keeps problem size constant; a soft penalty and softened acceleration-level HOCBF complement the hard constraint, and the full obstacle set checks between-node collisions.Source: §I · pp. 1–2
  • 58–67% lower position RMSE than the linear MPC planner and 41–70% lower than the polynomial planner; NMPP also succeeded in 86% of agile-profile flights versus SUPER’s 26%.In the super-fast forest profile, NMPP completed all flights without collision; it also avoided obstacles at speeds up to 5.5 m s−1 in real-world flight.Source: §I · p. 2

II. RELATED WORK

  • Global path planners and corridor-based optimizers typically require waypoints or a corridor and then generate a trajectory, while learned sensor-to-command methods require retraining when the platform or its parameters change.Corridors restrict motion to a convex subset of free space; the cited learned approaches reduce modular-pipeline latency but embed little prior vehicle knowledge.Source: §II · pp. 2–3
  • Predictive-control approaches connect local perception to planning, but prior methods simplify vehicle dynamics, use a separate safety filter, or represent obstacles with soft costs.NMPP instead uses hard obstacle constraints on full quadrotor dynamics with thrust dynamics, without a convex corridor, and provides a full-state reference with fixed problem size.Source: §II · p. 3

III. PROBLEM FORMULATION

The problem is online finite-horizon quadrotor planning toward waypoints in an initially unknown environment. The formulation describes the vehicle state, rotor allocation, and dynamics that constrain the planned motion.

III. PROBLEM FORMULATION

  • At each replanning instant, the planner generates a finite-horizon trajectory toward an active waypoint while respecting nonlinear dynamics, physical limits, and observed LiDAR obstacles.The environment is initially unknown, and the reference may be one waypoint or a sequence.Source: §III · p. 3
  • The optimization’s collision-free requirement applies to obstacles observed by onboard LiDAR, alongside the trajectory’s progress toward its reference waypoint.The planning objective is posed at each replanning instant in the initially unknown environment.Source: §III · p. 3

A. UAV Model Definition

  • The state is x = (p, q, v, ω), comprising position, unit-quaternion attitude, linear velocity, and body rate.The per-rotor thrust vector f is in R^4.Source: A. UAV Model Definition · p. 3
  • Rotor thrusts act on the body through an allocation matrix determined by rotor positions and the rotor torque constant κ.The allocation relates the rotor thrusts to collective thrust and body torque.Source: A. UAV Model Definition · p. 3
  • The dynamics use the rotation matrix R(q), UAV mass m, gravity g, and diagonal rigid-body inertia J to describe the quadrotor motion.The model denotes quaternion multiplication with ⊛.Source: A. UAV Model Definition · p. 3
  • The model includes velocity-dependent drag, fD = −KvR(q)⊤v, with diagonal coefficients kvx, kvy, and kvz.The drag coefficients form Kv = diag(kvx, kvy, kvz).Source: A. UAV Model Definition · p. 3

B. Nonlinear Model Predictive Control Formulation

The planner repeatedly solves a finite-horizon optimal control problem in a real-time-iteration scheme. Its formulation distinguishes exactly enforced constraints from softened constraints penalized through slack variables.

B. Nonlinear Model Predictive Control Formulation

  • The reference trajectory is generated by repeatedly solving a finite-horizon OCP over N shooting nodes with time steps Δt_k in a real-time-iteration scheme.The OCP is stated in the form used by acados.Source: B. Nonlinear Model Predictive Control… · p. 3
  • At each shooting node, the optimization uses control input u_k and discrete dynamics fRK4 obtained by fourth-order Runge–Kutta integration over Δt_k.The formulation uses the current state estimate x̂ and stage and terminal costs ℓ and ℓe.Source: B. Nonlinear Model Predictive Control… · p. 3
  • The constraint vector h is enforced exactly, while h_s is softened using slack variables penalized by a quadratic-and-linear function.A subset of softened constraints also applies at the terminal node, with separate slack variables and penalty Φe.Source: B. Nonlinear Model Predictive Control… · p. 3
  • Fig. 2 is identified as the overview of the complete NMPP system pipeline.The passage directs readers to Section IV for the stage cost, constraint sets, and choice of time steps.Source: B. Nonlinear Model Predictive Control… · p. 3, Figure 2 · p. 4

IV. PROPOSED METHOD

NMPP configures NMPC as a trajectory planner and sends its full-state reference to a separate controller. Its model and constraints are shaped to produce smoother references while enforcing vehicle limits.

A. Augmented NMPC Model

  • Using thrust rate rather than thrust as the control avoids acceleration jumps between shooting nodes, yielding p ∈ C2 and ω ∈ C1 references.The thrust is included in the state, and its rate is the control.Source: A. Augmented NMPC Model · p. 4

B. Constraints

  • Actuator constraints remain hard, while softened motion constraints permit temporary violations, such as after switching to a more restrictive profile.The thrust-rate formulation makes actuator limits linear bounds on the state and control.Source: B. Constraints · p. 4
  • Vertical acceleration bounds are enforced by tightening collective-thrust limits, and horizontal acceleration bounds by replacing the tilt limit.The replacement tilt limit is the largest tilt at which the upper thrust limit still produces at most axy,max.Source: B. Constraints · p. 4
  • The tilt limit uses a linear outer approximation alongside an exact condition; the gyroscopic term is neglected to keep the bound linear in the state.The paper says the neglected term is small relative to Mf in the considered operating regime.Source: B. Constraints · p. 4

C. Collision Avoidance

The collision-avoidance pipeline converts LiDAR measurements into a compact obstacle set for NMPC. It limits the optimization to a fixed number of relevant obstacles while retaining the full perceived set for validation.

C. Collision Avoidance

  • A fixed number of the most relevant obstacles enters the optimization, keeping its problem size fixed despite variable scene complexity.The complete perceived obstacle set is retained for subsequent trajectory validation.Source: C. Collision Avoidance · p. 4
  • The complete perceived obstacle set is retained for trajectory validation, beyond the obstacles selected for optimization.Source: C. Collision Avoidance · p. 4
  • LiDAR points are accumulated over non-overlapping 0.20 s windows to obtain denser obstacle-surface coverage.The onboard Livox MID-360 uses a non-repetitive scanning pattern.Source: 1) Point-Cloud Processing · p. 5

1) Point-Cloud Processing:

The method processes LiDAR points into obstacle clusters and represents clusters with oriented ellipsoids. Its obstacle representation models the ground separately and assumes locally level terrain.

1) Point-Cloud Processing:

  • Range-adaptive voxel downsampling preserves finer resolution near the UAV and uses coarser resolution farther away before Euclidean clustering.Source: 1) Point-Cloud Processing · p. 5
  • Each cluster is represented by one or more PCA-oriented ellipsoids enclosing its points, with an inflation margin for vehicle size.Ellipsoids provide tighter approximations than spheres for elongated structures such as tree trunks, poles, and walls.Source: 2) Obstacle Representation · p. 5

2) Obstacle Representation:

  • The ground is modeled as a horizontal plane with prescribed clearance, under a locally level-terrain assumption that excludes ground inclination and uneven terrain.Source: 2) Obstacle Representation · p. 5

3) Obstacle Constraints and Penalties:

Obstacle avoidance combines hard geometric constraints with a soft penalty and a softened acceleration-level condition. The geometric constraint remains hard, while the added terms encourage earlier avoidance and account for approach rate.

3) Obstacle Constraints and Penalties:

  • Each selected ellipsoid imposes a hard geometric constraint at prediction nodes, preventing feasible solutions from placing a node inside it.Source: 3) Obstacle Constraints and Penalties · p. 5
  • The bounded soft obstacle penalty increases smoothly near an ellipsoid boundary, encouraging avoidance before the hard constraint becomes active.Its activation distance and steepness are controlled by εobs and αobs.Source: 3) Obstacle Constraints and Penalties · p. 5
  • An acceleration-level condition uses the first two derivatives of the obstacle function to account for clearance and approach rate; it is softened while geometric clearance stays hard.No separate first-order HOCBF constraint is imposed for LiDAR obstacles.Source: 3) Obstacle Constraints and Penalties · p. 5

4) Obstacle Selection and Trajectory Validation:

Obstacle selection keeps the optimization bounded and validates trajectories against the active obstacle set. The cost function balances state tracking, thrust and control behavior, angular acceleration, and obstacle penalties across a nonuniform horizon.

4) Obstacle Selection and Trajectory Validation:

  • 4) Obstacle Selection and Trajectory Validation: Lowest-S_j obstacles form the active set; trajectories violating ellipsoid or minimum-altitude constraints are rejected, and prior predictions are reused only if collision-free.When obstacle count exceeds the fixed slot count, obstacles are ranked by minimum normalized separation from the remaining prediction.Source: 4) Obstacle Selection and Trajectory… · p. 5

D. Cost Function

  • D. Cost Function: The cost penalizes position, attitude, velocity, and body-rate tracking errors, plus thrust deviation, thrust-control changes, angular acceleration, and obstacle penalties.The terminal residual omits the control-dependent entries.Source: D. Cost Function · p. 6
  • D. Cost Function: Shorter time steps near the current state preserve integration accuracy, while longer late-horizon steps extend reach with a fixed number of shooting nodes.The time steps are nonuniform.Source: D. Cost Function · p. 6

E. Implementation Details

The implementation runs a fixed-dimension planner at 100 Hz and interpolates its stored prediction to provide commands at every control step. The constraint profiles and free-space tracking table describe the evaluation conditions and reported measures.

E. Implementation Details

  • The planner runs at 100 Hz with 28 shooting nodes spanning 2.0 s and 15 fixed obstacle slots, keeping problem dimensions constant during flight.The command is interpolated from the stored prediction at each control step; stale predictions are discarded after the horizon.Source: E. Implementation Details · p. 6
  • The constraint profiles specify limits, with the snap limit equal to the jerk limit in every profile.Table I lists the constraint profiles.Source: Table I · p. 6
  • Table II reports mean 3D RMSE for 20 m free-space flights across constraint profiles, peak-speed ranges across planners, and percentage error reductions relative to each baseline.No reported standard deviation exceeded 0.045 m.Source: Table II · p. 6

V. EXPERIMENTS AND RESULTS

Across the evaluated profiles, NMPP improves tracking error and achieves higher forest-flight success than SUPER under the reported aggressive settings. The simulations use matched controllers and vehicle models, and the stored prediction maintains continuous references despite occasional slow solves.

A. Experimental methodology

  • The simulations use the same unmodified SE(3) controller for planner comparisons, and each configuration is flown ten times or fifty times under the agile profile.The simulated vehicle uses Gazebo with an emulated PX4 autopilot; planners share the vehicle model and state estimator.Source: A. Experimental methodology · p. 6, B. Simulation Results · p. 6

1) Reference Tracking:

  • Across profiles, maximum tracking error ranges from 0.07 m to 0.55 m for NMPP, versus 0.12 m to 1.23 m for linear MPC and 0.10 m to 1.4 m for polynomial planning.The advantage is retained across all evaluated constraint profiles.Source: 1) Reference Tracking · pp. 6–7

2) Obstacle avoidance:

  • Mean NMPC solution time is 5.5 ms to 7.0 ms across forest profiles; some super-fast solves exceed 10 ms, mostly early in flight, without interrupting the reference.Stored-prediction interpolation keeps commands continuous; Fig. 3 shows per-update solution times for the super-fast profile.Source: 2) Obstacle avoidance · p. 7, Figure 3 · p. 7
  • Fig. 4 shows predicted paths against obstacles and red dashed safety margins; lines crossing distant obstacles were predicted before LiDAR scanned them.The plot covers ten super-fast simulation runs.Source: Figure 4 · p. 7, 2) Obstacle avoidance · p. 7
  • At super fast, NMPP succeeds in 100% of runs versus SUPER's 70%; at agile, NMPP reaches the goal in 43 of 50 flights versus 13 for SUPER.Peak speeds were 11.4 m s−1 for NMPP and 13.2 m s−1 for SUPER.Source: 2) Obstacle avoidance · p. 7, Table IV · p. 7

C. Real-world experiments

NMPP reduced tracking error and achieved strong forest-flight success in simulation, then flew a real obstacle course at speeds up to 5.5 m s−1 in gusty wind. Onboard solves exceeded the planner-loop period on average, while interpolation kept the reference continuous.

C. Real-world experiments

  • The real-world evaluation comprised ten flights through an approximately 40 m obstacle course under the fast profile, with gusts moving pylons and adding perception uncertainty..Source: C. Real-world experiments · p. 7
  • Mean onboard solution time was 13.5 ms, exceeding the 10 ms planner period, but stored-prediction interpolation kept the reference continuous.Table V summarizes performance over the ten real-world flights under the fast profile.Source: C. Real-world experiments · p. 7, Table V · p. 8

VI. CONCLUSION

  • The planner models UAV dynamics and actuation limits, producing dynamically feasible, collision-free references that an unmodified SE(3) controller can track closely..Source: §VI · pp. 7–8
  • Position RMSE was 58–67% lower than linear MPC and 41–70% lower than polynomial planning; agile forest success was 86% for NMPP versus 26% for SUPER.In the first four constraint profiles, NMPP reached the goal without collision in all ten flights per profile.Source: §VI · p. 8
  • Real-world flights avoided obstacles at speeds up to 5.5 m s−1 in gusty wind, with onboard computation.Fig. 5 shows the real UAV trajectories and corresponding speeds.Source: §VI · p. 8, Figure 5 · p. 8
Loading 2610.08695v1…