Trajectory Splitting: A Distributed Formulation for Collision Avoiding Trajectory Optimization

Changhao Wang, Jeffrey Bingham, Masayoshi Tomizuka

Abstract

Efficient trajectory optimization is essential for avoiding collisions in unstructured environments, but it remains challenging to have both speed and quality in the solutions. One reason is that second-order optimality requires calculating Hessian matrices that can grow with $O(N^2)$ with the number of waypoints. Decreasing the waypoints can quadratically decrease computation time. Unfortunately, fewer waypoints result in lower quality trajectories that may not avoid the collision. To have both, dense waypoints and reduced computation time, we took inspiration from recent studies on consensus optimization and propose a distributed formulation of collocated trajectory optimization. It breaks a long trajectory into several segments, where each segment becomes a subproblem of a few waypoints. These subproblems are solved classically, but in parallel, and the solutions are fused into a single trajectory with a consensus constraint that enforces continuity of the segments through a consensus update. With this scheme, the quadratic complexity is distributed to each segment and enables solving for higher-quality trajectories with denser waypoints. Furthermore, the proposed formulation is amenable to using any existing trajectory optimizer for solving the subproblems. We compare the performance of our implementation of trajectory splitting against leading motion planning algorithms and demonstrate the improved computational efficiency of our method.

Introduction

In robotics, finding a path that is both smooth and collision-free is a fundamental challenge. Current approaches typically fall into two categories: sampling-based planners and optimization-based planners. Sampling-based methods like RRT are fast and parallelizable but often produce suboptimal, "jerky" paths. Conversely, optimization-based planners like TrajOpt or CHOMP generate high-quality, smooth trajectories but suffer from high computational costs. Specifically, their complexity scales quadratically—$O(N^2)$—with the number of waypoints ($N$) due to the calculation of Hessian matrices.

Previous attempts to parallelize optimization often split the problem by constraint type (e.g., separating physics from collision checking). This frequently leads to load imbalances where one processor becomes a bottleneck. This paper introduces "Trajectory Splitting," a distributed framework that divides the path variables themselves to achieve better load balancing and computational efficiency.

Methodology: Trajectory Splitting via ADMM

The core innovation of this work is the decomposition of a long trajectory into $M+1$ segments. Each segment is treated as an independent subproblem with a reduced number of waypoints, significantly lowering the $O(N^2)$ complexity per sub-solver.

The ADMM scheme distributes subproblem solving across multiple agents, coordinated by a central unit that updates dual variables to ensure consensus.

To ensure these segments form a continuous, valid trajectory, the framework employs the Alternating Direction Method of Multipliers (ADMM). As shown in Figure 1, local agents solve their respective subproblems in parallel, while a central unit updates dual variables to enforce a consensus constraint at the segment boundaries. This formulation is optimizer-agnostic, allowing the use of standard solvers like IPOPT for the subproblems. By distributing the path variables, the computational load is naturally balanced across processors, as each segment typically encounters similar environmental complexity.

Experimental Evaluation

The proposed method was evaluated on a 2D toy problem and a 6-DOF FANUC LRMATE 200iD robot arm performing a "bookcase" task. The 6-DOF scenario involved picking a bottle and placing it on a shelf, a task requiring high-resolution waypoints to ensure collision avoidance in tight spaces.

Computation time as a function of splitting tolerance and the number of segments. TrajSplit configurations significantly outperform the non-distributed baseline as tolerance increases.

The results demonstrate a significant performance advantage. In the bookcase scenario, splitting the trajectory into three segments (TrajSplit_3) reduced the average computation time to 0.178s, a 2.7x speedup over the standard IPOPT baseline (0.489s). Furthermore, TrajSplit maintained superior path quality compared to sampling-based methods; its paths were approximately 7.4 rad in length, whereas RRT produced paths of ~11.2 rad. The method also achieved a high success rate (23/25), matching or exceeding other optimization-based planners while outperforming RRT* in time-constrained scenarios.

Discussion and Conclusion

Trajectory Splitting successfully addresses the quadratic complexity of trajectory optimization by distributing the computational load across segments. While the method shows great promise, it is sensitive to hyperparameters such as the number of segments and splitting tolerance. A loose tolerance can lead to minor discontinuities at split points, which might be problematic for very thin obstacles. Future work will explore "Fast ADMM" variants and alternative subproblem optimizers to further enhance convergence speed and robustness. In conclusion, this distributed formulation provides a scalable path toward real-time, high-quality motion planning for complex robotic systems.

Summary updated

Abstract

Efficient trajectory optimization is essential for avoiding collisions in unstructured environments, but it remains challenging to have both speed and quality in the solutions. One reason is that second-order optimality requires calculating Hessian matrices that can grow with $O(N^2)$ with the number of waypoints. Decreasing the waypoints can quadratically decrease computation time. Unfortunately, fewer waypoints result in lower quality trajectories that may not avoid the collision. To have both, dense waypoints and reduced computation time, we took inspiration from recent studies on consensus optimization and propose a distributed formulation of collocated trajectory optimization. It breaks a long trajectory into several segments, where each segment becomes a subproblem of a few waypoints. These subproblems are solved classically, but in parallel, and the solutions are fused into a single trajectory with a consensus constraint that enforces continuity of the segments through a consensus update. With this scheme, the quadratic complexity is distributed to each segment and enables solving for higher-quality trajectories with denser waypoints. Furthermore, the proposed formulation is amenable to using any existing trajectory optimizer for solving the subproblems. We compare the performance of our implementation of trajectory splitting against leading motion planning algorithms and demonstrate the improved computational efficiency of our method.
BibTeX
@inproceedings{wang2021trajectory,
  author = {Changhao Wang and Jeffrey Bingham and Masayoshi Tomizuka},
  title = {{Trajectory Splitting: A Distributed Formulation for Collision Avoiding Trajectory Optimization}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2021},
  url = {https://openreview.net/forum?id=0lOK6i4QSH},
}
Figure