Source-linked AI summary
Search-based Motion Planning for Aggressive Flight in SE(3)
Sikang Liu, Kartik Mohta, Nikolay Atanasov, Vijay Kumar
TL;DR
The paper addresses aggressive quadrotor flight through clutter while accounting for attitude-dependent collision constraints that spherical models omit. It combines motion-primitive graph search with hierarchical refinement, and demonstrates dynamically feasible trajectories in simulation and real-world experiments, while noting control-model limitations during tracking.
Problem
Existing spherical or prism models simplify collision checking but do not capture quadrotor shape, attitude, and under-actuated dynamics in cluttered environments.
Method
The planner searches motion primitives with attitude-aware collision checking and uses lower-dimensional trajectories as heuristics for higher-dimensional refinement.
Results
The planner generates dynamically feasible trajectories for aggressive flight and demonstrates them in simulations and real-world experiments.
Takeaways & Limitations
Accounting for attitude and body shape allows the quadrotor to plan safe trajectories through constrained clutter while retaining search-based optimality in the discretized space.
Takeaways & Limitations
Real-robot tracking shows attitude-control lag, and a more accurate model would use snap rather than jerk as the control input.
Abstract
from arXiv · showhide
Quadrotors with large thrust-to-weight ratios are able to track aggressive trajectories with sharp turns and high accelerations. In this work, we develop a search-based trajectory planning approach that exploits the quadrotor maneuverability to generate sequences of motion primitives in cluttered environments. We model the quadrotor body as an ellipsoid and compute its flight attitude along trajectories in order to check for collisions against obstacles. The ellipsoid model allows the quadrotor to pass through gaps that are smaller than its diameter with non-zero pitch or roll angles. Without any prior information about the location of gaps and associated attitude constraints, our algorithm is able to find a safe and optimal trajectory that guides the robot to its goal as fast as possible. To accelerate planning, we first perform a lower dimensional search and use it as a heuristic to guide the generation of a final dynamically feasible trajectory. We analyze critical discretization parameters of motion primitive planning and demonstrate the feasibility of the generated trajectories in various simulations and real-world experiments.
I. INTRODUCTION
The paper develops a planner for aggressive quadrotor flight that accounts for the robot’s shape, attitude, and under-actuated dynamics in cluttered environments. It combines motion-primitive search with hierarchical refinement to produce dynamically feasible trajectories while addressing limitations of spherical models.
- Motivation: Quadrotor motion planning is challenging because attitude and dynamics prevent simple geometric collision conditions.Existing approaches often approximate the robot as a sphere or prism, simplifying collision checking but not capturing attitude-dependent feasibility.
- Approach: The planner models the quadrotor’s actual shape and computes attitude constraints along dynamically feasible trajectories.This enables trajectories that pass through doors narrower than the robot’s diameter, unlike spherical-body models.
- Motivation: Under-actuation couples translation and rotation, making planners designed for fully actuated systems unsuitable for quadrotors.The paper therefore builds on motion primitives that discretize the control space while preserving dynamic feasibility.
- Approach: A hierarchical refinement process uses lower-dimensional trajectories as heuristics to accelerate planning in higher-dimensional state and control spaces.The contributions also analyze how motion-primitive discretization affects computation time, smoothness, and optimality.
- System model: The system dynamics map desired acceleration and gravity to orientation, while desired jerk determines angular velocity.Yaw is assumed constant because it is decoupled from the system dynamics in the planning model.
B. Search-based Planning using Motion Primitives
The planner constructs a graph of polynomial motion primitives generated from a finite control set and searches it for a minimum-cost trajectory. An LQMT-based heuristic accounts for trajectory smoothness while guiding A* search toward the goal.
- Primitive generation: A motion primitive applies a constant control input from a finite set UM for duration τ, producing a polynomial trajectory between two states.For jerk control, the state contains position, velocity, and acceleration, and the resulting primitive is minimum-jerk between its endpoints.
- Graph construction: The finite control set UM and duration τ define a graph G(S, E) whose states are reachable states and whose edges are motion primitives.Applying each control element iteratively generates the graph explored by breadth-first search over a finite horizon.
- Cost and search: The planner searches for a trajectory minimizing control effort and time, with ρ setting their trade-off.Each primitive has effort J = ∥um∥2τ and duration T = τ, so its cost depends on the input and primitive duration.
- Cost and search: An A* search recovers an optimal trajectory from the initial state to the goal region over the motion-primitive graph.The individual primitive cost is independent of the current state and depends only on UM and τ.
- Heuristic: The LQMT heuristic underestimates cost-to-go by relaxing dynamics and obstacle constraints while retaining trajectory-smoothness considerations.This avoids relying solely on distance, which can miss the velocity, acceleration, or orientation changes required by dynamic trajectories.
C. Feasibility Checking
Feasibility checking must enforce both dynamic limits from the quadrotor model and geometric collision constraints from physical obstacles.
- Feasibility constraints: A motion primitive is feasible only when it satisfies both dynamical constraints and geometric constraints imposed by obstacles.The planner checks whether the entire primitive remains within the free space while respecting the system’s physical limits.
1) Dynamically Feasible Primitives:
The planner enforces dynamic feasibility by bounding velocity, acceleration, and jerk independently along each axis and checking those bounds over each primitive interval.
- Dynamical constraints: Differential flatness enables componentwise velocity, acceleration, and jerk constraints instead of explicitly applying the quadrotor’s full nonlinear motor constraints.This provides a tractable approximation of the motor-induced thrust and torque limits.
- Dynamical constraints: Polynomial derivatives allow closed-form checking of minimum and maximum velocity, acceleration, and jerk over t ∈ [0, τ].The bounds are obtained by finding roots of the corresponding derivatives, ensuring planned trajectories remain within the specified limits.
2) Collision Free Primitives:
The planner checks collision freedom by modeling the quadrotor as an attitude-dependent ellipsoid and testing sampled states along each motion primitive against obstacle points.
- Traditional spherical occupancy inflation is conservative for agile trajectories because it ignores the quadrotor’s attitude during collision checking.
- The quadrotor body is represented as an ellipsoid whose position and attitude are obtained from the trajectory state and acceleration-dependent orientation.
- Collision checking tests whether the ellipsoid intersects the obstacle point cloud, requiring an empty intersection for a state to be feasible.
- A KD-tree first crops nearby obstacle points within radius r around the ellipsoid center, assuming r ≥ h, to reduce collision-checking cost.
- Because an ellipsoid’s contour along a primitive is nonconvex, the planner samples I states and declares the primitive collision-free only when every sampled state has no obstacle intersection.
IV. TRAJECTORY REFINEMENT
Trajectory refinement uses a lower-dimensional plan to guide higher-dimensional motion-primitive search, reducing computation time while trading away direct optimality. Increasing control-space order raises execution and computation time, with computation growing dramatically.
- Trajectory refinement: Higher continuity requirements increase state-space dimension, making direct high-dimensional planning more time- and memory-intensive.For C2 continuity, jerk control yields a second-order state space of R9.
- Control-space comparison: Using velocity, acceleration, or jerk controls, execution time and computation time both increase with control-space order.The ordering is T1 < T2 < T3 and t1 < t2 < t3.
- Heuristic guidance: A lower-dimensional trajectory supplies waypoints and a heuristic that focuses higher-dimensional search near the prior trajectory instead of the whole state space.The heuristic is inadmissible by itself, but multi-heuristic A* can combine it with a consistent LQMT heuristic to preserve optimality.
- Heuristic guidance: The refined trajectories have greater combined control-effort and execution-time cost than direct optima, but require much less computation time.This comparison applies when Φ1 guides planning of Φ2 and Φ3.
V. EVALUATION
Two-dimensional planning can guide a quadrotor through progressively narrower wall gaps by increasing its desired roll angle. The smallest passable gap is approximately 0.525 m under the stated robot geometry and planning assumptions.
- 2-D planning: As a wall gap shrinks, jerk-controlled trajectories increase the desired roll angle at the gap.The experiments vary the gap by moving one wall closer to the other.
B. 3-D Planning
The planner extends gap traversal into 3-D by adding z-axis control, allowing smaller gaps and trajectories through inclined rectangular openings while accounting for roll and pitch. Planning performance depends substantially on motion-primitive and control-space discretization choices.
- 3-D gap traversal: Adding z-axis control relaxes the 2-D gap constraint and permits arbitrary roll and pitch angles while reducing the traversable gap width.The reported setting uses maximum acceleration amax = g.
- Gap-width experiments: Gap experiments compare 2-D and 3-D trajectories across widths from 0.75m to 0.35m, with φgap denoting maximum roll at the gap.The top figures show 2-D results and the bottom figures show 3-D results.
- Inclined rectangular openings: 0.4 × 0.8m rectangular holes at different orientations require non-zero roll and pitch for a robot with radius r = 0.35m.The planner generates agile trajectories through these inclined openings.
- Planning parameters: Motion-primitive duration τ and control-space discretization affect graph density, exploration effort, and search failure risk.For moderate flight speeds, τ = 0.2s is reported as a reasonable choice; smaller values densify the graph, while larger values can leave it too sparse.
VI. EXPERIMENTS
Experiments evaluate the planner in cluttered simulated environments and on a real quadrotor. Hierarchical refinement reduces planning computation and explored states, while the real vehicle tracks aggressive trajectories through narrow gaps but exhibits attitude-control lag.
- Simulation results: Hierarchical planning uses an acceleration-controlled trajectory Φ2 to guide jerk-controlled trajectory Φ3 and requires much less computation than direct 9-dimensional jerk planning.The refined search also explores fewer states, although its trajectory is not optimal.
- Simulation results: The refined trajectory Φ3* has higher cost than the optimal trajectory Φ3.This cost difference accompanies the reduced exploration observed during refinement.
- Simulation results: Table I evaluates trajectory-generation computation time t, total jerk effort J, and execution time T.These metrics characterize both planning cost and the resulting trajectory.
- Real-world experiments: The Hummingbird quadrotor follows planned trajectories through cluttered environments and a narrow gap using VICON localization, depth-based mapping, and robust feedback control.The narrow-gap flight reaches a maximum roll angle of 40°.
- Real-world experiments: The robot tracks velocity properly up to 4m/s, but attitude control shows lag because specified angular velocity cannot be achieved instantly.The paper identifies snap, rather than jerk, as a more accurate control input for the quadrotor model.
- Conclusion: The conclusion presents the planner as enabling aggressive SE(3) flight with attitude constraints while claiming safety, smoothness, and optimality guarantees.Future work targets integration with onboard sensing, state estimation, and feedback control for autonomous flight in unknown clutter.
Linear Quadratic Minimum Time for Jerk Control
The planner constructs a cost-to-go heuristic by solving the minimum-time, minimum-control-effort jerk trajectory between a current state and a goal. The heuristic is obtained by minimizing the trajectory cost over its duration.
- Heuristic construction: The heuristic H(s, sg) underestimates the actual cost by relaxing dynamics and obstacle constraints.It is used to guide graph search toward the goal.
- Optimal trajectory: The initial state is s = [p0, v0, a0]T, and Pontryagin’s minimum principle provides the optimal trajectory formulation.The trajectory coefficients are determined by the state conditions, goal state, and duration T.
- Minimum-time solution: The total cost C(T) is minimized by differentiating with respect to T and finding a root over T ∈ [0, ∞).The resulting optimal duration is T*, and the heuristic is H(s, sg) = C(T*).
- Coefficient cases: The coefficients in the derivative equation are defined for the fully specified goal state sg = [p1, v1, a1]T.The supplied coefficient fragments include c6 = −3600(p0 − p1)2 and c6 = −1600(p0 − p1)2 for different cases.