Paper · 2017-10-07 · cs.RO
Search-based Motion Planning for Aggressive Flight in SE(3)
Plans aggressive quadrotor flight through gaps narrower than the robot's diameter. The body is an ellipsoid tilted by the planned acceleration, and a graph search over constant-jerk primitives checks it for collisions. A lower-dimensional plan guides the jerk search. Author-reported: 2.0 s and 24.0 s to refine, against 89.4 s and 129.6 s for direct jerk search, in two simulated scenes.
Motion primitivesTime-optimal controlA* searchmotion-planningquadrotorcollision-checkingtrajectory-optimization
arXivPDFCodeMarkdown13 min read
AI-compiled by claude-haiku-5-5 on 2026-11-13, not yet reviewed by a human. Every claim cites its source; check it when a detail matters.
In plain words
A quadrotor is a small flying robot with four rotors. It has to tilt to accelerate sideways, so its tilt matters when it passes a narrow gap. A planner that treats the robot as a ball has to leave wide margins, which wastes most of the room the gap offers. This work models the robot as an ellipsoid, a stretched ball, tilted the way the robot would be flying, so the planner can see that the robot fits through a gap if it rolls a little while passing.
The planner builds a path out of short pieces of smooth motion, each one a fixed jerk (change in acceleration) held for a short time. It searches over sequences of these pieces, like a route finder over a grid of moves. A quick search with a simpler model first finds a rough route, and that route then steers a slower, more accurate search. The aim is a flight that is quick, smooth and safe.
TL;DR
Search over constant-jerk motion primitives for a quadrotor whose body is an ellipsoid tilted by the desired acceleration, so narrow gaps can be passed with attitude. A lower-dimensional search gives a heuristic for the full jerk search. Author-reported timings in two simulated scenes are about 2 s and 24 s for the refined search, against 89 s and 130 s for the direct jerk search. A short real-flight test passes a gap with about 40 degrees of roll §VI.B.
Problem
Planners usually model a quadrotor as a sphere or a prism and inflate obstacles by the robot size. Under that model the robot becomes a point, but the model is conservative: it rejects trajectories that would be feasible once attitude is taken into account §I. Planning in full SE(3), with translation and rotation coupled, is needed for agile flight §II. Sampling planners are slow to converge and give unpredictable intermediate results, and weighted graph search can lose optimality §II. The authors want a deterministic, smooth, dynamically feasible and collision-free trajectory, with the robot's real shape and attitude counted §I.
Method
The work extends the authors' earlier motion-primitive planner, which assumed a spherical body §III. The steps are below.
-
Attitude from the desired motion. The thrust direction is the desired acceleration plus gravity eq. 1. The third body axis is that vector, normalised, and the other two axes follow from a heading ψ eq. 2 eq. 3. The heading is taken as constant during planning, because yaw does not change the translational dynamics §III.A. The angular velocity is derived from the jerk eq. 4 eq. 5 eq. 6.
-
State and primitives. The planner uses the flat output position x(t), so the state is s = (p, v, a) in R^9 and the control is jerk §III.A eq. 9. A primitive applies a constant jerk u for a duration τ from state s0 and gives a cubic-in-time position, quadratic velocity and linear acceleration eq. 10. The authors state that this primitive is the minimum-jerk curve between the two states §III.B.
-
Graph and objective. A finite control set U_M and duration τ define a graph whose states are reached by applying each control in turn §III.B fig. 2. The optimal trajectory minimises the integral of squared jerk plus ρ times the total time eq. 11. A primitive's cost is (|u|² + ρ)τ, which depends only on u and τ eq. 12. Finding the optimal trajectory is then a deterministic shortest-path problem over controls, with feasibility constraints eq. 13. The authors solve it with a graph search such as A* §III.B.
-
Heuristic. The distance heuristic is replaced by a closed-form linear-quadratic minimum-time (LQMT) cost H(s, s_g), which accounts for smoothness §III.B. For jerk control its derivation is in the appendix: the cost of a time-optimal curve is a polynomial in the duration T, and H is its value at the root T* of the derivative app. A eq. 29 eq. 30. Three coefficient sets are given, for a fully defined goal, a goal with position and velocity, and a goal with position only eq. 31 eq. 32 eq. 33.
-
Dynamic feasibility. Velocity, acceleration and jerk are bounded componentwise on each axis, and the extrema on [0, τ] are found in closed form by solving for roots of the derivatives §III.C.1 eq. 15.
-
Collision. The body at a state is the ellipsoid with centre x(t) and shape E = R diag(r, r, h) Rᵀ, where r is the radius, h the height, and R the attitude from step 1 §III.C.2 eq. 16 eq. 17. Obstacles are a point cloud. A k-d tree crops points within radius r of the centre, assuming r ≥ h §III.C.2. The primitive is accepted when no point lies inside the ellipsoid at I sampled states eq. 18 eq. 19. The sampling is needed because the swept contour is not convex §III.C.2.
-
Hierarchical refinement. A trajectory Φ^p is first planned with a lower-dimensional control (velocity or acceleration), then a higher-dimensional search Φ^q is guided by H = H1 + H2 §IV.B eq. 22. H1 is the LQMT term from the current state to the prior waypoint at the same time index app. A. H2 is the prior's remaining time times ρ, since the prior's effort is zero in the lower space eq. 23 eq. 24. The heuristic is not admissible. The authors propose to combine it with the consistent LQMT heuristic in multi-heuristic A* to regain optimality §IV.B.
The search loop below is our summary of steps 2 to 5. It is not code from the paper.
# Best-first search over constant-jerk primitives [§III.B] [eq. 13]
# Input: start state s0 = (p, v, a), goal region X_goal,
# control set U_M, primitive duration tau
# Output: control sequence u_0 .. u_{N-1}, or failure
def search_primitives(s0, goal_region, controls, tau):
# Cost so far per state; the heuristic only orders the frontier [§III.B]
cost_so_far = {s0: 0.0}
frontier = priority_queue()
frontier.push(s0, key=heuristic(s0))
while frontier is not empty:
# Expand the state with the smallest key [§III.B]
state = frontier.pop_min()
if state in goal_region:
# End state inside the goal region [eq. 13]
return controls_along_path(state)
for u in controls:
# Primitive: constant jerk u for tau from state [eq. 10]
successor = primitive_end_state(u, state, tau)
# Discard infeasible or colliding primitives [§III.C] [eq. 15]
if not primitive_is_feasible(u, state, tau):
continue
# Edge cost (|u|^2 + rho) * tau [eq. 12]
step_cost = (dot(u, u) + rho) * tau
new_cost = cost_so_far[state] + step_cost
if new_cost < cost_so_far.get(successor, infinity):
cost_so_far[successor] = new_cost
frontier.push(successor, key=new_cost + heuristic(successor))
return failureAttitude and the collision test, as described in steps 1 and 6:
# Body rotation from the desired acceleration [§III.A] [eq. 1] [eq. 2]
# Input: acc (3,) desired acceleration; g gravity; psi heading (held const)
# Output: rotation matrix R (3, 3) with columns r1, r2, r3
def body_rotation(acc, g, psi):
# Desired thrust vector: acceleration plus gravity [eq. 1]
force = acc + g * z_world
# Third body axis is the normalised thrust [eq. 2]
r3 = force / norm(force)
# Heading vector in the horizontal plane [eq. 3]
r2c = (-sin(psi), cos(psi), 0)
# First and second body axes complete the frame [eq. 2]
r1 = normalise(cross(r2c, r3))
r2 = cross(r3, r1)
return (r1, r2, r3)# Body ellipsoid at a sampled state [§III.C.2] [eq. 16] [eq. 17]
# Input: position (3,), acc (3,), radius r, height h
# Output: centre and shape matrix E of the ellipsoid
def body_ellipsoid(position, acc, r, h):
R = body_rotation(acc, g, psi)
# Semi-axes r, r and h along the body frame [eq. 17]
E = R * diag(r, r, h) * transpose(R)
return position, E# Collision test for one primitive [§III.C.2] [eq. 18] [eq. 19]
# Input: primitive, obstacle points (N, 3), radius r, height h, I samples
# Output: True if no obstacle point is inside any sampled ellipsoid
def primitive_is_collision_free(primitive, obstacles, r, h, I):
for i in range(I):
# State at the i-th sample time along the primitive [eq. 19]
centre, shape = body_ellipsoid(
primitive_position(primitive, i),
primitive_acc(primitive, i),
r,
h,
)
# Only points within radius r of the centre can be inside [§III.C.2]
for point in points_within(obstacles, centre, r):
if inside_ellipsoid(point, centre, shape):
return False
return TrueHyperparameters
Values are from the paper unless marked as code. Items the paper does not give are written "not stated".
| Name | Value | Where | Notes |
|---|---|---|---|
| ρ (time weight) | 10000 | §V.C | Stated as ρ ≈ 4 u_max²; a larger ρ gives faster trajectories §V.C |
| τ (primitive duration) | 0.2 s | §V.C | "Reasonable for moderate flight speeds"; the Fig. 2 graph uses τ = 0.5 s fig. 2 |
| v_max | 7 m/s | §V.C | Bound in the closed-form check eq. 15 |
| a_max | 10 m/s² | §V.C | The 2-D analysis instead sets a_max = g §V.A |
| u_max (jerk range) | 50 m/s³ | §V.C | Upper end of the control set |
| du (control step) | 12.5 m/s³ | §V.C | Not used by the code; see Paper vs code |
| robot radius r | 0.35 m | §V.A fig. 7 | Gap analysis; the simulations use 0.5 m fig. 9 |
| body height h | 0.1 m | §V.A | Ellipsoid height, 2-D analysis |
| gravity g | not stated | §III.A | Code uses 9.81 m/s² |
| yaw ψ | not stated | §III.A | Held constant during planning |
| samples per primitive I | not stated | §III.C.2 | Code derives it; see Pitfalls |
| number of controls |U_M| | not stated | §III.B | Fig. 2 shows 9 and 25 controls; grid size is in the code |
| goal region tolerances | not stated | §III.B | Code sets 2.0 m, 2.0 m/s and 100 m/s² ellipsoid_planner_node.cpp |
| weight ε of the weighted search | not stated | — | Code default 1.0; shipped example 2.0 |
| timing hardware | not stated | §V.C tab. I | Computation times have no machine stated |
Shipped office example (code, not the paper's analysis set): v_max 10, a_max 10, u_max 60, dt 0.2, num 2, w 10000, epsilon 2, robot_r 0.5, use_3d false test.launch.
Results
Author-reported. Setup: simulated office and unstructured 3-D maps, each a point cloud with a KD-tree of obstacles §VI.A tab. I. Baseline: the same search in the full 9-dimensional jerk space tab. I. The computation times below are for each search on its own tab. I.
| Setting | Metric | Direct jerk search (Φ³) | Refined jerk search (Φ³*) | Source |
|---|---|---|---|---|
| Office | computation time t | 89.42 s | 2.03 s (plus 9.34 s for Φ²) | tab. I |
| Office | total jerk effort J | 8.9 × 10³ | 11.1 × 10³ | tab. I |
| Office | execution time T | 4.6 s | 5.0 s | tab. I |
| Unstructured 3-D | computation time t | 129.58 s | 24.02 s (plus 21.64 s for Φ²) | tab. I |
| Unstructured 3-D | total jerk effort J | 5.6 × 10³ | 15.1 × 10³ | tab. I |
| Unstructured 3-D | execution time T | 3.0 s | 4.8 s | tab. I |
- Trajectories planned with velocity, acceleration and jerk controls for one start and goal take 2 ms, 60 ms and 1646 ms, with execution times 32 s, 33 s and 34 s fig. 4.
- With the velocity plan as the heuristic, the acceleration and jerk searches take 11 ms and 98 ms, with execution times 35 s and 36 s fig. 6.
- 2-D planning gives an approximate smallest passable gap of 0.525 m for r = 0.35 m §V.A. See Pitfalls for the arithmetic.
- Figure 7 shows trajectories through gaps from 0.75 m down to 0.35 m: the top row is 2-D planning, the bottom row 3-D planning with a_max = g fig. 7 §V.B.
- A window test at three inclinations (30°, 45°, 60°) shows trajectories through a 0.4 × 0.8 m hole fig. 8.
- Real flight: an AscTec Hummingbird with a VICON system and a depth-sensor map passes a gap with about 40° of roll §VI.B fig. 11. Velocity tracking holds up to 4 m/s, with visible attitude lag §VI.B fig. 12.
Paper vs code
Checked against the authors' repository at code_commit. Code citations abbreviate the directory as .../: the headers are in the mpl_external_planner package under ellipsoid_planner/, and the node and launch file are in the mpl_test_node package. The motion-primitive library (mpl_planner, mpl_basis) that holds the primitive, cost and heuristic code is not in the repository, so those parts were not read.
- Ellipsoid frame. Consistent. The code builds the third axis from acceleration plus gravity, as in eq. 1 and eq. 2, and uses the same shape matrix R diag(r, r, h) Rᵀ as eq. 17 primitive_ellipsoid_utils.h. The first two axes come from a fixed x-axis rather than the heading in eq. 3. This does not change the shape, because the ellipsoid is symmetric in those two axes primitive_ellipsoid_utils.h.
- Gravity. The paper writes g and gives no number. The code uses 9.81 m/s² in the body frame and in the attitude check primitive_ellipsoid_utils.h primitive_ellipsoid_utils.h. The 2-D analysis sets a_max = g §V.A, while the table gives a_max = 10 m/s² §V.C.
- Samples per primitive. The paper says I samples and does not give I. The code uses n = ceil(max axis velocity × τ / r) and then tests n + 1 samples, both endpoints included ellipsoid_util.h primitive_ellipsoid_utils.h. The paper's own ranges also disagree: eq. 19 takes i = 0 … I − 1, while the display after eq. 20 takes i = 0 … I §III.C.2.
- Radius for the obstacle search. Consistent. The code searches within radius r, as the paper does, under the same assumption r ≥ h ellipsoid_util.h ellipsoid_util.h.
- Weighted search. The paper does not mention ε and speaks of optimal trajectories §VI.A §VII. The node defaults to ε = 1.0 ellipsoid_planner_node.cpp ellipsoid_planner_node.cpp, but the shipped office example sets ε = 2 test.launch.
- Control set. The paper gives du = 12.5 m/s³ §V.C. The code builds the control set from u_max and a count num, with step u_max / num, so du = 30 in the shipped example (num = 2, u_max = 60) ellipsoid_planner_node.cpp. The code's control set is planar unless use_3d is set ellipsoid_planner_node.cpp.
- Goal region. The paper names a goal region but gives no tolerances §III.B. The node sets 2.0 m, 2.0 m/s and 100 m/s² ellipsoid_planner_node.cpp.
- Prior trajectory. Not in the paper. When a prior trajectory is used, the node sets goal use_acc and use_jrk to false, which relaxes the goal condition ellipsoid_planner_node.cpp. The example runs with use_prior false test.launch.
- Parameter set. The analysis table §V.C and the shipped example differ: v_max 7 vs 10 m/s, u_max 50 vs 60 m/s³, robot radius 0.35 vs 0.5 m test.launch. The 0.5 m radius matches the simulation figure fig. 9.
- Jerk limit. The paper's closed-form check uses a jerk bound j_max eq. 15. The node sets only the velocity and acceleration limits ellipsoid_planner_node.cpp. The value of j_max is not visible in the repository, because it lives in the library not included.
Pitfalls
- The heuristic is not admissible, and the optimality claim depends on it. The paper says H is inadmissible §IV.B and proposes multi-heuristic A* to regain optimality, without spelling it out. The shipped example uses weighted search with ε = 2, so its results are not guaranteed optimal in the sense of eq. 13. The refined trajectories have higher effort than the direct jerk search (11.1 × 10³ vs 8.9 × 10³ in office; 15.1 × 10³ vs 5.6 × 10³ in unstructured) tab. I.
- Two searches, and the timings are split. Table I lists the lower-dimensional search (Φ²) and the refined search (Φ³*) separately. Adding them gives 11.37 s (office) and 45.66 s (unstructured), still below the direct search, but the paper does not state whether the prior's time should be counted.
- The gap formula does not reproduce. The 2-D limit is approximately 2r cos θ with θ not defined, and the text gives 0.525 m §V.A. With r = 0.35 m and the roll bound arctan(a_max/g) = 45° from eq. 25, 2r cos 45° = 0.495 m, not 0.525 m.
- Safety holds only at sampled states. Collision is tested at I sampled states, not along the whole primitive. The value of I is not stated, and the paper calls the contour non-convex §III.C.2. Between samples, a clash can go unseen.
- Bounding box checked at two points. The code's bounding-box test calls
sample(2)and checks only those points ellipsoid_util.h. A primitive that leaves the box between two points would pass, unlesssample(2)in the missing library means something else. This could not be checked. - Obstacle filter runs before the box is set. The environment constructor calls
setObstaclesbeforesetBoundingBoxenv_cloud.h. The filter insidesetObstaclestherefore tests against an empty box ellipsoid_util.h. Whether an empty polyhedron accepts every point is in the missing library, so the effect is unverified. - Continuous states need a rule for revisits. The paper does not say how two nearly identical states are merged in the search. An implementation must choose a tolerance or discretisation, and the results depend on it.
- Attitude assumes ideal tracking. Attitude is computed from the desired acceleration, "ignoring feedback control errors" §III.A. The real flight shows roll lag §VI.B fig. 12, so the attitude that clears an obstacle in the plan may not be the one flown.
- Discretisation sets success. A small τ makes the graph dense and slow, a large τ can leave it too sparse to find a path, and a coarse control set makes the search less complete and the trajectories less smooth §V.C.
- Figure and text disagree on τ. Fig. 2 uses τ = 0.5 s, the text recommends 0.2 s fig. 2 §V.C.
- Timing hardware not stated. The computation times in Table I cannot be compared with another machine, because no hardware is given tab. I.
- Yaw is fixed. Yaw is held constant throughout planning §III.A. The ellipsoid is symmetric in its first two axes, so the choice of yaw does not change the collision test.
Lineage
- Builds on: the authors' IROS 2017 planner, which uses lattice search over motion primitives generated by optimal control and an LQMT heuristic §I §III.B. The reference list gives it as a 2017 IROS paper by Liu, Atanasov, Mohta and Kumar on quadrotor planning with LQMT control; no arXiv id is given in the source, and it has no card. Jerk primitives and the Pontryagin solution come from Mueller et al., 2015 §III.B app. A.
- Compared against: velocity- and acceleration-control planning within the paper fig. 4 fig. 6; direct jerk search in Table I tab. I. No external planner is compared.
- Superseded by: not stated.
Source and license
Adapted from “Search-based Motion Planning for Aggressive Flight in SE(3)” by Sikang Liu, Kartik Mohta, Nikolay Atanasov, Vijay Kumar (arXiv:1710.02748), used under CC BY 4.0. Changes: condensed and restructured for implementation, with tables, equations and algorithms taken from the paper and its code, each cited to its source. Not affiliated with or endorsed by the authors. Code citations refer to https://github.com/sikang/mpl_ros at commit 155014cab848 (Apache-2.0). Any quoted code is shown under that license.