PaperlayerView exact revision on arXiv

Source-linked AI summary

Motion Planning around Obstacles with Convex Optimization

Tobia Marcucci, Mark Petersen, David von Wrangel, Russ Tedrake

arXiv:2205.04422v1cs.RO

TL;DR

The paper tackles collision-free trajectory planning around obstacles, where direct optimization can fail in cluttered spaces and sampling-based methods face tradeoffs. It combines Bézier curves with Graphs of Convex Sets to formulate a compact mixed-integer problem, then uses convex relaxation and randomized rounding. Across several robotic settings, GCS typically identifies globally optimal or high-quality trajectories efficiently and provides optimality bounds, while higher-order derivative penalties and some additional constraints remain incompletely supported.

  • Problem

    Collision-free planning with obstacles remains difficult because direct trajectory optimization is nonconvex and can fail in clutter, while planners must balance dimensionality, dynamics, computation, completeness, and optimality.

  • Method

    GCS combines Bézier-curve trajectory parameterization with shortest paths in Graphs of Convex Sets, solving a compact MICP through convex relaxation, randomized rounding, and a small convex refinement program.

  • Results

    GCS finds globally optimal solutions in the reported tasks, with certified relaxation gaps averaging 4.1% and reaching 13.0% in the first task, while outperforming PRM with higher-quality trajectories in less time.

  • Takeaways & Limitations

    Convex optimization can reliably support collision-free motion planning in high-dimensional settings, including quadrotor flight and fourteen-dimensional dual-arm manipulation.

  • Takeaways & Limitations

    Tight convex approximations for penalties on second and higher derivatives are still under development, and future work targets broader costs, input-limit dynamics, task-space constraints, and contact planning.

Abstract

from arXiv · show

Trajectory optimization offers mature tools for motion planning in high-dimensional spaces under dynamic constraints. However, when facing complex configuration spaces, cluttered with obstacles, roboticists typically fall back to sampling-based planners that struggle in very high dimensions and with continuous differential constraints. Indeed, obstacles are the source of many textbook examples of problematic nonconvexities in the trajectory-optimization problem. Here we show that convex optimization can, in fact, be used to reliably plan trajectories around obstacles. Specifically, we consider planning problems with collision-avoidance constraints, as well as cost penalties and hard constraints on the shape, the duration, and the velocity of the trajectory. Combining the properties of Bézier curves with a recently-proposed framework for finding shortest paths in Graphs of Convex Sets (GCS), we formulate the planning problem as a compact mixed-integer optimization. In stark contrast with existing mixed-integer planners, the convex relaxation of our programs is very tight, and a cheap rounding of its solution is typically sufficient to design globally-optimal trajectories. This reduces the mixed-integer program back to a simple convex optimization, and automatically provides optimality bounds for the planned trajectories. We name the proposed planner GCS, after its underlying optimization framework. We demonstrate GCS in simulation on a variety of robotic platforms, including a quadrotor flying through buildings and a dual-arm manipulator (with fourteen degrees of freedom) moving in a confined space. Using numerical experiments on a seven-degree-of-freedom manipulator, we show that GCS can outperform widely-used sampling-based planners by finding higher-quality trajectories in less time.

1 Introduction

The paper addresses continuous collision-free motion planning with obstacles, where existing approaches trade off dimensionality, dynamics, runtime, completeness, and optimality. It introduces GCS, a convex-optimization-based planner that uses Bézier curves and Graphs of Convex Sets to efficiently plan high-dimensional trajectories.

  • Direct trajectory optimization handles high-dimensional kinematics and dynamics but can fail to find collision-free paths in cluttered configuration spaces.Its nonconvex formulation and reliance on local optimization create the difficulty.
  • Sampling-based planners provide probabilistic completeness but are used as a fallback when direct optimization struggles.The passage frames this guarantee as coming with computational costs.
  • GCS targets a limited but important class of differential-constraint problems and is designed to solve very high-dimensional instances in a few seconds through a single convex program.The paper demonstrates the planner on mazes, buildings, and a fourteen-dimensional dual-arm manipulation task.
  • GCS formulates obstacle avoidance over safe convex regions as a shortest-path problem in Graphs of Convex Sets.The regions are connected through an adjacency graph, with trajectory segments assigned to regions.
  • Bézier curves enable convex collision-avoidance formulations that are typically second-order-cone programs.This avoids the expensive semidefinite constraints required by existing MICP planners for higher differentiability.

2 Problem Statement

The paper defines motion planning as optimizing trajectory duration, length, and velocity energy subject to collision avoidance, differentiability, velocity, duration, and endpoint constraints. A path-coordinate and time-scaling transformation separates trajectory shape from duration for finite-dimensional convex transcription.

  • The resulting infinite-dimensional problem is intended for finite-dimensional transcription and practical convex programming.The paper later parameterizes r and h using Bézier curves.
  • The abstract planning problem minimizes a weighted sum of duration, trajectory length, and velocity energy.The weights are user-specified nonnegative values.
  • Collision avoidance is enforced by requiring the trajectory to remain in safe convex regions at all times.This is stronger than checking collision only at finitely many sampled points.
  • The formulation also imposes differentiability, convex velocity limits, bounded positive duration, and endpoint constraints.These constraints jointly encode the trajectory's regularity, speed, timing, and start-end conditions.
  • A fixed path coordinate s and monotonically increasing time-scaling function h separate trajectory shape from its duration.The original trajectory is represented through path variables r and time scaling h.

3 Background on B´ezier Curves

Bézier curves provide a finite parameterization whose Bernstein coefficients form convex combinations of control points. Their endpoint, convex-hull, derivative, and convex-integration properties support trajectory constraints and convex optimization.

  • Bézier curves parameterize trajectories using a finite set of control points and Bernstein polynomials.The Bernstein coefficients are nonnegative and sum to one.
  • A Bézier curve starts at its first control point and ends at its last control point.This gives direct control over trajectory endpoints.
  • The entire Bézier curve lies in the convex hull of its control points.This property allows geometric containment to be enforced through control-point constraints.
  • The derivative of a degree-d Bézier curve is a degree-(d−1) Bézier curve with control points d(γk+1 − γk).Derivatives therefore retain the same curve structure and remain expressed through finite variables.
  • Integrating a convex function along a Bézier curve can be bounded using the function values at its control points.The result follows from convexity and the Bernstein-polynomial integral.

4 The Optimization Framework

GCS casts trajectory design as a shortest-path problem over convex sets, formulates it as a compact mixed-integer convex program, and solves its relaxation with randomized rounding. The rounded path is refined by a small convex program, while repeated trials improve solution quality.

  • 4.1 Shortest Paths in Graphs of Convex Sets: A Graphs-of-Convex-Sets shortest-path problem combines a directed graph, convex vertex sets, continuous vertex points, convex edge lengths, and endpoint-coupling constraints.The path is discrete, while the points and edge costs are continuous.
  • 4.1 Shortest Paths in Graphs of Convex Sets: The GCS formulation minimizes the sum of edge lengths over a valid source-to-target path.Only variables associated with visited vertices are constrained by the path-specific convex conditions.
  • 4.2 Rounding the Convex Relaxation of the Shortest-Path Problem: The shortest-path problem is formulated as a compact MICP with a tight convex relaxation, which the paper solves instead of using exact branch-and-bound.The relaxation is followed by a randomized recovery of an approximate discrete path.
  • 4.2 Rounding the Convex Relaxation of the Shortest-Path Problem: Relaxed edge variables in [0,1] are interpreted as transition probabilities for randomized depth-first search with backtracking.The search traverses outgoing unvisited edges according to normalized relaxed values.
  • 4.2 Rounding the Convex Relaxation of the Shortest-Path Problem: After a path is selected, a small convex program recovers its continuous variables and cost.The method can return an infeasible continuous assignment with infinite cost, so multiple trials are evaluated and the lowest-cost rounded solution is retained.

5 Collision-Free Motion Planning using Graphs of Convex Sets

The planner transcribes collision-free motion planning into a shortest-path problem over convex safe regions, assigning Bézier trajectory and time-scaling curves to selected regions. Tight convex relaxations and rounding enable efficient trajectory reconstruction while preserving collision avoidance, boundary conditions, velocity limits, and differentiability up to conservative bounds.

  • Bézier parameterization: Each selected region carries Bézier curves r_i for trajectory shape and h_i for time scaling, with control points collected in x_i.The curve degree satisfies d ≥ η + 1, where η is the required differentiability order.
  • Rounding and optimality: Randomized rounding of the convex relaxation handles symmetrically equivalent paths better than greedy edge selection, and a relaxation-matching path certifies global optimality.When a path cost equals C_relax, the path is globally optimal.
  • Graph construction: Each safe region Q_i becomes a graph vertex, with edges connecting intersecting regions and source/target edges enforcing q_0 and q_T.The source σ and target τ encode initial and terminal conditions without requiring decision variables.
  • Convex constraints: Control-point containment in Q_i guarantees collision-free trajectory segments through the Bézier convex-hull property.Positive control points of the time-scaling derivative make h_i strictly increasing, while velocity control constraints enforce the robot’s velocity set.
  • Edge constraints: Boundary edge constraints impose initial and terminal positions, velocities, and admissible duration, while internal edge constraints enforce differentiability across segments.These constraints are linear in the Bézier control-point variables.
  • Trajectory reconstruction: The shortest-path formulation reconstructs the full trajectory by sequencing the Bézier curves associated with the regions selected by the optimal path.The resulting functions satisfy the original continuity, collision-avoidance, velocity, and monotonic-time requirements up to conservative constraints and cost bounds.

6 Penalties on the Higher-Order Derivatives of the Trajectory

Higher-order derivative penalties are generally nonconvex when trajectory shape and time scaling are optimized jointly. The paper therefore uses regularization based on derivative penalties and a minimum time-scaling rate to smooth trajectories while minimally affecting cost.

  • Limitation: Convex penalties on higher time derivatives become nonconvex when trajectory shape r and time scaling h are optimized jointly.Such penalties are relevant to limiting control effort for manipulators and quadrotors.
  • Second derivative: Acceleration magnitude can be controlled by increasing ḣ_min and penalizing the magnitudes of r̈ and ḧ under a bounded velocity set D.The resulting regularization is a convex surrogate rather than a direct convex formulation of acceleration cost.
  • Higher derivatives: Higher derivatives q^(m) are regularized by increasing ḣ_min and penalizing derivatives r^(l) and h^(l) for l = 2, …, m.The approach follows from applying Faà di Bruno’s formula to the composite trajectory.
  • Effect: Numerical results show that these regularization terms can sensibly smooth trajectories while only minimally affecting their cost.The paper notes that these terms are less tight than the velocity bounds.

7 Numerical Results

Across two-dimensional scenes, mazes, quadrotor environments, and manipulators with up to 14 degrees of freedom, GCS produced globally optimal or near-optimal trajectories with compact convex optimization. It also outperformed PRM baselines in trajectory length and runtime while certifying optimality bounds.

  • 7.2 Maze: In a 2,500-cell maze, minimum-length and minimum-time objectives selected different cell sequences, while a single SOCP certified both solutions as globally optimal.The convex relaxation required no rounding because it returned integral transition probabilities.
  • 7.3 Statistical Analysis: Quadrotor Flying through Buildings: 95% of quadrotor environments yielded trajectories within 1% of the global optimum, with a worst-case cost only 2.9% above the global minimum.GCS automatically certified gaps below 4% for 68% of problems and below 7% for 84%; the largest certified gap was 27.1%.
  • 7.4 Comparison with PRM: Motion Planning of a Robot Arm: For the seven-degree-of-freedom robot arm, GCS produced shorter trajectories than both PRM variants and ran faster than regular PRM across all five tasks.PRM with short-cutting generated higher-quality trajectories than regular PRM but required significantly more runtime.
  • 7.5 Coordinated Planning of Two Robot Arms: GCS scaled to a 14-dimensional dual-arm manipulator, with certified relaxation gaps of 3.3%, 2.0%, and 0.6% and computation times of 4.0 s, 8.4 s, and 12.9 s.Mixed-integer verification established global optimality for the first two tasks and a 0.3% optimality gap for the third.

8 Discussion

GCS combines convex optimization with Bézier trajectory parameterization and a Graphs of Convex Sets formulation to plan efficiently around obstacles, while trading modeling flexibility for tractable, reliable optimization.

  • Strengths and limitations: GCS uses a Graphs of Convex Sets shortest-path formulation to design trajectories around obstacles through efficient convex optimization.Its advantages include tight convex relaxations, fewer binary variables, and a simple resulting optimization class.
  • Additional costs and constraints: Joint optimization of trajectory shape and timing makes continuous-time dynamics constraints nonconvex, while nonlinear manipulator kinematics hinder direct task-space constraints.Barrier penalties and local nonconvex post-processing are proposed as practical workarounds for some applications.
  • Comparison with existing mixed-integer planners: GCS achieves tight relaxations and fewer binary variables by leveraging the optimization framework from [26].Its simple optimization class also depends partly on Bézier trajectory parameterization.
  • Comparison with existing mixed-integer planners: Bézier curves enforce infinite families of trajectory constraints through finitely many control-point conditions.For safety, requiring each control point r_i,k to lie in Q_i enforces r_i(s) ∈ Q_i for all s ∈ [0, 1].
  • Comparison with existing mixed-integer planners: Compared with SOS-based formulations, Bézier nonnegative-control-point conditions are more stringent than univariate SOS nonnegativity conditions.The two approaches are interchangeable in formulation, but involve a tradeoff.
  • Comparison with sampling-based algorithms: GCS can outperform PRM in runtime, trajectory quality, scalability with configuration-space dimension, and the variety of objectives and constraints.GCS generalizes PRM by expanding collision-free samples into convex regions and compressing the roadmap into a compact graph.
  • Comparison with sampling-based algorithms: GCS prioritizes low runtimes and planning completeness over the modeling power of direct trajectory optimization.Direct optimization can include broader cost and constraint classes but relies on slow and unreliable local optimization in practice.

9 Conclusions and Future Works

The paper introduces GCS as a convex-optimization planner for efficient collision-free motion planning. Across maze, quadrotor, and high-dimensional manipulation scenarios, it finds higher-quality trajectories in less time than widely used PRM methods, while future work targets broader constraints and applications.

  • Conclusions: GCS formulates collision-free motion planning as a tight, lightweight convex optimization, typically an SOCP, followed by cheap randomized rounding.The rounding is almost always sufficient to identify a globally optimal trajectory.
  • Conclusions: GCS was demonstrated in an intricate maze, a quadrotor-building scenario, and a fourteen-dimensional manipulation task.The paper also compares GCS with widely used PRM methods.
  • Conclusions: GCS finds higher-quality trajectories in less time than widely used PRM methods.This comparison is reported across the demonstrated planning scenarios rather than as a single task-specific metric.
  • Future works: Future work includes faster customized solvers, broader cost and constraint classes, task-space and dynamic constraints, and contact-rich planning.The authors identify these extensions as targets for expanding GCS's practical scope.

A Further Details on the Implementation of GCS

The appendix describes techniques used in the numerical experiments to tighten and compress the convex relaxations of GCS planning problems.

  • Implementation: The implementation appendix presents two techniques for tightening and compressing GCS convex relaxations.These techniques were employed in the numerical results in Section 7.

A.1 Two-Cycle-Elimination Constraints

Two-cycle elimination strengthens GCS relaxations by excluding mutually incompatible directed edges between overlapping safe regions and tightening their coupling with continuous variables.

  • Two-Cycle-Elimination Constraints: For overlapping safe regions, the graph contains a two-cycle whose two directed edges cannot both belong to a path.Thus, at least one of the corresponding indicator variables must be excluded from the shortest path.
  • Two-Cycle-Elimination Constraints: Linear constraints encode the incompatibility using edge and vertex flow variables, implying the looser condition ϕ_e + ϕ_f ≤ 1.The vertex flows ϕ_i and ϕ_j represent total probability flows through vertices i and j.
  • Two-Cycle-Elimination Constraints: Applying Lemma 1(b) translates the two flow inequalities into convex constraints that tighten coupling between flow variables and continuous variables x_v.These constraints are added for each pair of overlapping regions.

A.2 Graph Pre-Processing

The pre-processing stage removes graph redundancies before optimization by identifying edges that cannot belong to any valid source-to-target path. Exact inference is infeasible in general, so the method uses a fast multiflow-based sufficient test, with exact decomposition details for the building examples.

  • Graph Pre-Processing: Exact edge-redundancy checking is infeasible in general because it is equivalent to an NP-complete vertex-disjoint-paths problem.The test requires disjoint subpaths from the source to one endpoint and from the other endpoint to the target.
  • Graph Pre-Processing: A fractional multiflow LP relaxation provides a fast sufficient condition for detecting redundant edges.The multiflow LPs can be solved quickly and used instead of exact preprocessing.
  • Graph Pre-Processing: The approximate preprocessing can eliminate most graph redundancies in many practical scenarios.It is particularly useful for sparse, small graphs whose associated convex sets lie in high dimensions, where it can drastically compress and tighten the optimizations.
  • Graph Pre-Processing: For the random-building environments, polygonal walls and trees permit exact configuration-space decomposition into box-shaped safe regions.The safe regions are shrunk to account for collision geometry.

C Implementation of the PRM Planner

The comparison PRM planner uses a two-stage roadmap construction because uniformly sampling valid seven-dimensional robot poses is infeasible. RRTs first build a graph-guided skeleton, after which the roadmap and shortcutting parameters balance path quality against computation time.

  • Implementation: 3 · 10^5 samples and 90 hours of computation found no robot pose with the end effector inside a shelf.The difficulty motivates constructing the roadmap in two stages rather than sampling poses directly.
  • Implementation: The first stage connects seed poses with bidirectional RRTs to form a compact PRM skeleton.RRT connections are added only for seed-pose pairs whose corresponding vertices are connected in graph G.
  • Implementation: 12 RRTs produce approximately 2,300 nodes while following the connectivity of graph G.This graph-guided construction keeps the roadmap skeleton reasonably compact.
  • Implementation: The RRT, PRM, and shortcutting parameters are chosen to trade off path quality against total computation time.The stated criterion is overall computational efficiency rather than either objective alone.
Loading 2205.04422v1…