PaperlayerView exact revision on arXiv

Source-linked AI summary

MADER: Trajectory Planner in Multi-Agent and Dynamic Environments

Jesus Tordesillas, Jonathan P. How

arXiv:2010.11061v2cs.ROcs.MA

TL;DR

MADER addresses real-time 3D trajectory planning around static obstacles, dynamic obstacles, and other planning agents. It combines MINVO outer polyhedral representations with separating-plane optimization and decentralized asynchronous collision checking. Simulations report lower flight time and fewer stops than Bernstein/B-Spline bases, with favorable comparisons to centralized and synchronous decentralized approaches.

  • Problem

    Real-time 3D trajectory planning that handles static obstacles, dynamic obstacles, and other planning agents remains an open problem.

  • Method

    MADER uses MINVO-based outer polyhedral trajectory representations, optimizes separating planes, and incorporates committed agent trajectories with collision check-recheck in decentralized asynchronous planning.

  • Results

    MADER reports up to 33.9% lower flight time and 88.8% fewer stops than Bernstein/B-Spline bases, plus shorter distances than centralized approaches and shorter average total times than synchronous decentralized approaches.

  • Takeaways & Limitations

    The results support MADER as a decentralized asynchronous planner for collision-free UAV trajectories in environments containing static obstacles, dynamic obstacles, and other agents.

Abstract

from arXiv · show

This paper presents MADER, a 3D decentralized and asynchronous trajectory planner for UAVs that generates collision-free trajectories in environments with static obstacles, dynamic obstacles, and other planning agents. Real-time collision avoidance with other dynamic obstacles or agents is done by performing outer polyhedral representations of every interval of the trajectories and then including the plane that separates each pair of polyhedra as a decision variable in the optimization problem. MADER uses our recently developed MINVO basis to obtain outer polyhedral representations with volumes 2.36 and 254.9 times, respectively, smaller than the Bernstein or B-Spline bases used extensively in the planning literature. Our decentralized and asynchronous algorithm guarantees safety with respect to other agents by including their committed trajectories as constraints in the optimization and then executing a collision check-recheck scheme. Finally, extensive simulations in challenging cluttered environments show up to a 33.9% reduction in the flight time, and a 88.8% reduction in the number of stops compared to the Bernstein and B-Spline bases, shorter flight distances than centralized approaches, and shorter total times on average than synchronous decentralized approaches.

SUPPLEMENTARY MATERIAL

MADER addresses real-time 3D trajectory planning with static obstacles, dynamic obstacles, and other agents through decentralized asynchronous optimization. Its MINVO-based representations and collision-checking framework improve efficiency while maintaining safety constraints.

  • MADER targets the open problem of 3D real-time planning with static obstacles, dynamic obstacles, and other planning agents.
  • MINVO produces minimum-volume outer polyhedral representations, with volumes 2.36 and 254.9 times smaller than Bernstein and B-Spline representations.The minimum-volume guarantee applies in both position and velocity space.
  • MADER imposes separating planes between trajectory polyhedra as optimization variables, reducing the conservatism of fixed convex decompositions.The planes separate each pair of outer polyhedral representations for trajectory intervals.
  • Its decentralized asynchronous framework constrains optimization with agents’ committed trajectories and uses collision check-recheck to ensure safety during replanning.
  • 33.9% lower flight time and 88.8% fewer stops are reported relative to Bernstein/B-Spline bases, alongside shorter distances than centralized approaches and shorter average total times than synchronous decentralized approaches.

II. DEFINITIONS

The paper defines agents, obstacles, and cubic clamped B-Splines, then states assumptions governing prediction, communication, asynchronous replanning, and obstacle geometry.

  • An agent can exchange information and make decisions, whereas an obstacle moves without considering other elements and may be static or dynamic.The paper uses “dynamic obstacles” for what some literature calls non-cooperative agents.
  • MADER uses clamped uniform cubic B-Splines with n + 1 control points, m + 1 knots, and intervals bounded by four control points.Each interval is a degree-3 polynomial contained within the convex hull of its four control points.
  • The terminal goal is projected onto a sphere centered on d for simplicity, with an obstacle-avoiding piecewise-linear alternative also described.
  • Obstacle prediction and trajectory discretization errors, together with discretization step sizes, are assumed known to construct outer polyhedral approximations.
  • Agents are assumed to communicate without delay, share a reference time, trigger replanning asynchronously, and never commit new trajectories simultaneously.The non-simultaneous commitment assumption supports the safety argument for committing after checking prior trajectories.
  • Obstacles are assumed not to rotate, although MADER could account for rotation using an AABB containing all orientations over an interval.

IV. POLYHEDRAL REPRESENTATIONS

MADER represents each trajectory interval with tight polyhedra, using MINVO control points for position and velocity and converting between basis representations through known linear functions.

  • IV. POLYHEDRAL REPRESENTATIONS: Tight polyhedral outer representations encode every interval of the optimized, agent, and obstacle trajectories while avoiding infinitely many separation constraints.The representations support collision constraints over continuous trajectory intervals rather than only sampled points.
  • IV. POLYHEDRAL REPRESENTATIONS: MINVO volumes are 2.36 and 254.9 times smaller than Bernstein and B-Spline volumes in position space, and 1.29 and 5.19 times smaller in velocity space.The comparison is made for each interval’s outer polyhedral representation.
  • IV. POLYHEDRAL REPRESENTATIONS: For each interval, MINVO and B-Spline control points are related through known linear transformations whose matrices are available for the respective bases.The corresponding position and velocity mappings use interval-dependent B-Spline matrices where applicable.

B. Polyhedral Representation of the trajectory of other agents

For other agents and obstacles, MADER constructs interval-specific outer polyhedra by inflating bounding boxes and taking convex hulls of boxes placed along MINVO control points or obstacle positions.

  • B. Polyhedral Representation of the trajectory of other agents: For each interval, MADER inflates an agent’s bounding box by ηs and takes the convex hull of boxes placed at the relevant MINVO control points.The resulting polyhedron represents the agent trajectory interval, with vertices denoted Cij.
  • B. Polyhedral Representation of the trajectory of other agents: Other agents’ trajectories are partitioned according to the optimized agent’s intervals, even when their initial and final times differ.MINVO control points are computed for every portion of the other agent’s trajectory that falls within each matching interval.
  • C. Polyhedral Representation of the trajectory of the obstacles: For obstacles, MADER inflates each box by ηs+2(βij+αij), places it at the obstacle positions over the interval, and takes the convex hull of all box vertices.Under the stated assumption, this outer approximation covers the obstacle’s occupied 3D space during the interval, including agent-size inflation.
  • C. Polyhedral Representation of the trajectory of the obstacles: Static obstacles use the same construction as dynamic obstacles, with a constant position function.An example of these polyhedral representations is shown in Figure 4.

B. Other constraints

MADER combines boundary, dynamic-obstacle, and motion-limit constraints to produce trajectories that stop at the final time while avoiding obstacle polyhedra.

  • B. Other constraints: The initial position, velocity, and acceleration are imposed through x(tin) = xin, fixing the first three control points.The corresponding control points are therefore excluded from the decision variables.
  • B. Other constraints: The final stop condition imposes v(tf) = 0 and a(tf) = 0, requiring the last three control points to coincide.The final position is instead encouraged through a penalty cost weighted by ω ≥0.
  • B. Other constraints: The final-position penalty avoids infeasibility that can arise when the estimated total time is too short to reach the goal.The paper contrasts this penalty with imposing the goal as a hard constraint.
  • B. Other constraints: MADER computes outer polyhedral representations for dynamic-obstacle trajectory segments and requires each optimized trajectory segment to avoid the corresponding polyhedron.Figure 5 illustrates this construction for a box-shaped obstacle following a trefoil knot trajectory.
  • B. Other constraints: Velocity and acceleration limits are imposed independently on the x, y, and z axes using MINVO velocity control points and shared acceleration control points.The B-Spline and MINVO acceleration control points are identical.

C. Control effort

The section formulates the B-Spline trajectory optimization problem, including control effort based on constant jerk in cubic intervals, and solves a nonconvex problem over control points and separating planes.

  • C. Control effort: Cubic B-Spline evaluation uses an interval-specific normalized parameter and position matrix.The normalized parameter is defined over each knot interval.
  • C. Control effort: Constant jerk in each cubic interval defines the control-effort term.The constancy follows from using p = 3.
  • C. Control effort: The optimization jointly selects B-Spline control points and separating planes for collision-free trajectory constraints.The MINVO control points are linear transformations of the B-Spline decision variables.
  • C. Control effort: The nonconvex problem is solved with an augmented Lagrangian method and globally convergent MMA through NLopt.The trajectory time allocation is chosen before optimization from the distance between the goal and initial destination.

E. Initial Guess

Octopus Search constructs an initial guess by exploring B-Spline control points with feasible velocity samples, collision-separation checks, goal proximity, and runtime-oriented pruning.

  • E. Initial Guess: Search nodes are prioritized by f = g + ϵh, combining cost-to-come and heuristic distance-to-goal.Lower-f nodes are explored first, following the A*-inspired ordering.
  • E. Initial Guess: Octopus Search initializes q0, q1, and q2 from the incoming position, velocity, and acceleration.These initial control points seed the priority-queue search.
  • E. Initial Guess: Uniformly sampled velocities must satisfy vmax and amax before generating successor control points.Nodes are discarded when no feasible velocity samples remain.
  • E. Initial Guess: Candidate nodes are pruned using MINVO linear-separability tests against obstacle or agent intervals.The search also checks terminal conditions and the distance to the goal.
  • E. Initial Guess: Voxel-based proximity pruning keeps the search computationally tractable by rejecting control points near previously queued points.A new point is added only when no earlier point lies within the same voxel.
  • E. Initial Guess: When the goal is reached, the algorithm returns the control points and separating planes; otherwise it expands the queue with new control points.The closest trajectory found can represent the initial guess in dynamic-obstacle scenarios.

F. Degree of the splines

The paper focuses on cubic splines, while noting that MADER can extend to other spline orders with corresponding higher-order control objectives and MINVO polyhedra.

  • F. Degree of the splines: The paper focuses on p = 3, corresponding to cubic splines.This choice balances UAV dynamic feasibility and computational tractability.
  • F. Degree of the splines: Higher- or lower-order splines could use corresponding MINVO polyhedra and objectives such as snap minimization for p = 4.The paper describes this as a possible extension rather than the evaluated setting.

VI. DECONFLICTION

MADER deconflicts asynchronously through committed-trajectory constraints, collision check-recheck periods, and fallback execution rules that preserve safety during replanning.

  • VI. DECONFLICTION: MADER divides asynchronous deconfliction into Optimization, Check, and Recheck periods.The scheme addresses trajectories committed while another agent is optimizing.
  • VI. DECONFLICTION: During Optimization, agents include outer polyhedral representations of other agents’ trajectories as constraints and store newly committed trajectories.The Check period tests these newly committed trajectories for collisions.
  • VI. DECONFLICTION: The Recheck period uses a Boolean flag to detect trajectories received during collision checking without requiring an unbounded recheck loop.This keeps the final consistency check computationally simple.
  • VI. DECONFLICTION: Runtime is bounded by allocating κδt to initial-guess search and µδt to nonconvex optimization, with κ + µ < 1.The remaining time supports Check and Recheck; fallback solutions are selected when either computation exceeds its allocation.
  • VI. DECONFLICTION: Under the assumption that agents do not commit simultaneously, the deconfliction scheme guarantees safety with respect to other agents.The example illustrates that commitment order can differ from optimization start and finish order.
  • VI. DECONFLICTION: If a new plan is committed, it is checked against trajectories committed during optimization; otherwise, the agent continues its previous collision-free trajectory.An agent waits at the final stop if it reaches the previous trajectory’s endpoint before finding a new feasible solution.

VII. RESULTS

Single-agent simulations show that MINVO improves UAV speed, reduces stops, and shortens goal-reaching time as obstacle density increases.

  • Single-agent simulations: 4.15 m/s was the mean MINVO velocity, compared with 3.23 m/s for Bernstein and 2.79 m/s for B-Spline at vmax = 5 m/s.The comparison used a corridor-like environment containing 100 dynamic obstacles.
  • Single-agent simulations: 86.4% and 88.8% fewer stops were achieved with MINVO than with Bernstein and B-Spline, respectively.The experiment varied obstacle counts from 50 to 250 and treated stopping as unsafe around dynamic obstacles.
  • Single-agent simulations: 22.3% and 33.9% shorter goal-reaching times were achieved with MINVO than with Bernstein and B-Spline, respectively.The reported improvements are attributed to tighter outer polyhedral approximations in velocity and position spaces.
  • Single-agent simulations: Figure 12 reports goal-reaching time and stop counts across basis choices and obstacle counts using five simulations per combination.The shaded regions represent 1σ intervals, with σ denoting standard deviation.

B. Multi-Agent simulations without obstacles

In obstacle-free multi-agent simulations, MADER is compared with centralized and decentralized planners using decentralization, replanning, real-time, synchronization, and safety criteria.

  • Safety criteria: Discretized inter-agent constraints may be unsafe between sampled points, while finer discretization can impose very high computation times.The paper contrasts this limitation with continuous-time polyhedral outer representations.
  • Algorithm comparison: MADER has shorter overall flight distance and total time than the centralized safe-trajectory solution RBP.Table IV reports mean time and distance over five runs for SCP and MADER, with safety ratio taken as the minimum across runs.
  • Algorithm comparison: MADER has the shortest total time among decentralized algorithms except decNS_Search with u = 5 m/s^3.In that exception, MADER’s flight distance is 6.3 m shorter, while decNS_Search does not satisfy replanning real-time constraints.
  • Algorithm comparison: MADER is the only evaluated algorithm that is decentralized, supports replanning, satisfies replanning real-time constraints, and is asynchronous.The comparison used eight agents swapping positions in an 8 × 8 m square.
  • Replanning performance: MADER’s average replanning profile comprises 12 successful replans, 2.87 ms for checking, 0.034 µs for rechecking, and 199.6 ms total replanning time per UAV.Approximately half of the replanning time is allocated to finding the initial guess.

C. Multi-Agent simulations with static and dynamic obstacles

MADER is evaluated in circle and sphere environments combining static obstacles, dynamic obstacles, and multiple UAVs, with reported safety and performance results.

  • Multi-agent environments: The circle environment contains 25 static pillars and 25 dynamic boxes, while the sphere environment contains 18 static pillars and 52 dynamic obstacles.In both environments, UAVs swap positions while flying from circle or sphere formations.
  • Multi-agent environments: All reported inter-agent safety ratios exceed 1 in the circle and sphere environments.The environments include 25 static and 25 dynamic obstacles in the circle case, and 18 static plus 52 dynamic obstacles in the sphere case.
  • Multi-agent results: In the sphere environment, each UAV stops on average 0.125, 0.125, and 1.0 times with 8, 16, and 32 agents, respectively.The corresponding passage also reports circle-environment averages of 0.188 and 1.5 stops for its tested agent counts.
  • Overall results: The conclusion reports that MADER’s MINVO outer-polyhedral volumes are 2.36 and 254.9 times smaller than Bernstein and B-Spline volumes, respectively.The paper also states that simulations improved stops, computation or execution time, and flight distance relative to state-of-the-art algorithms.
  • Overall results: MADER combines decentralized asynchronous planning with committed-trajectory constraints and collision check-recheck safety enforcement.Future work includes perception-aware and risk-aware objective terms and hardware experiments.
Loading 2010.11061v2…