Joint Sampling and Trajectory Optimization over Graphs for Online Motion Planning

Kalyan Vasudev Alwala, Mustafa MukadamIROS 2021 · DOI

TL;DR

This paper introduces JIST, a novel motion planning approach that combines sampling and trajectory optimization within a factor graph framework. JIST excels in dynamic environments by efficiently exploring multiple potential paths and adapting to changes, outperforming existing sampling-only and optimization-only methods.

What Problem Does This Paper Address?

Planning efficient and safe motion for robots in real-world environments is a significant challenge, especially when these environments are large, dense with obstacles, and constantly changing (dynamic). Traditional methods often fall into two camps: sampling-based approaches (like RRT) that explore the environment but can be slow and struggle with tight constraints, and optimization-based methods that are fast and good at handling constraints but can get stuck in local optima and don't explore enough. This leaves a gap for methods that can effectively handle the complexities of dynamic, large-scale environments in real-time. The authors aim to bridge this gap by creating a planner that can both explore a wide range of possibilities and quickly refine them, adapting to changes as they happen.

Methods

The core of JIST is its use of factor graphs as a flexible data structure. These graphs represent the motion planning problem as a network of variables (robot states) and factors (constraints and objectives like collision avoidance, smoothness, and goal reaching).

JIST operates in an iterative loop:

  1. Grow FactorGraph: A sampling strategy, inspired by RRT, is used to add new states and edges to the factor graph. Crucially, JIST doesn't perform collision checking during this sampling phase. Instead, it relies on the optimization step to resolve potential collisions. This significantly speeds up exploration.
  2. Optimize: The entire factor graph (including newly sampled parts) is then optimized using a nonlinear least-squares approach. This step refines the sampled trajectories, moving them out of collisions and towards optimality. Because the graph can hold multiple potential paths (hypotheses), this optimization step effectively explores and refines several possibilities simultaneously.
  3. Execute and Prune: The robot executes a step from the best-found path. The robot's actual measured state is then used to prune parts of the factor graph that are no longer relevant or reachable, preparing for the next iteration.

This cycle allows JIST to continuously explore new possibilities, track multiple promising paths, and adapt to environmental changes efficiently.

Datasets

The paper evaluates JIST on several benchmark environments:

  • 2D Static: A static environment with uniformly spaced square obstacles.
  • 2D Forest: A dynamic 2D forest with randomly moving square obstacles.
  • Patrol: A dynamic narrow passage with moving rectangular obstacles.
  • Pedestrian: A realistic simulation in Gazebo with a ClearPath Ridgeback robot navigating around dynamic pedestrians.
  • 3D Forest: A 3D environment with a Sawyer robot arm manipulating objects in the presence of dynamic cubical obstacles.

The authors also provide detailed descriptions of the robots and their constraints used in these environments.

Key Insights and Ideas

  • Decoupling Sampling and Collision Checking: By deferring collision checking to the optimization phase, JIST significantly speeds up the exploration process, allowing it to build larger and more diverse hypothesis spaces than traditional sampling methods.
  • Factor Graphs as a Unified Backbone: Using factor graphs allows JIST to seamlessly integrate sampling (exploration) and optimization (exploitation) within a single, flexible framework. This is key to its ability to track and switch between multiple potential solutions.
  • Dynamic Adaptation: The iterative grow-optimize-prune cycle enables JIST to react to dynamic changes in the environment by replanning and adapting its trajectory in real-time.
  • Balancing Exploration and Exploitation: JIST effectively balances the broad search capabilities of sampling with the precise refinement of optimization, leading to better performance in challenging dynamic scenarios.

Key Results

JIST consistently outperformed its baselines (a sampling-only baseline, SBL, and an optimization-only baseline, OBL) across various metrics and environments.

  • Success Rate: JIST achieved significantly higher success rates in reaching the goal, especially in dynamic and complex environments where baselines often failed or collided. For instance, in the 2D Forest benchmark with 80 dynamic obstacles, JIST had a success rate of 0.43, while OBL was 0.20 and SBL was 0.03.
  • Execution Time: While JIST's total execution time was sometimes higher than OBL (due to optimization of a more complex graph), it was often faster than OBL in achieving a successful outcome and significantly faster than SBL in complex scenarios.
  • Compute Time: JIST's compute time per iteration was generally comparable to or slightly higher than OBL, but much lower than SBL, indicating efficient planning.
  • Qualitative Results: Figures and descriptions show JIST successfully navigating complex scenarios where baselines failed, demonstrating its robustness in dynamic environments.

Limitations and Future Work

  • Theoretical Guarantees: By not performing collision checking during sampling, JIST sacrifices some of the theoretical guarantees (like probabilistic completeness) offered by traditional sampling methods.
  • 3D SDF Computation: For 3D environments, precomputing Signed Distance Fields (SDFs) is done. The authors suggest that online GPU-based SDF computation could further improve real-time performance.
  • Sampling Strategies: While JIST uses an RRT-like sampler, the framework is flexible. Future work could explore goal-directed sampling, learned heuristics, or learned sampling distributions to further improve efficiency and bias exploration towards desired outcomes.
  • Optimization: Similarly, the optimization component could be enhanced with learned cost functions or weights.
  • Perception and State Estimation: The paper mentions using noisy state measurements and limited visibility. Improving robustness to perception errors and state estimation noise is an ongoing area of research.

Summary updated

BibTeX
@inproceedings{alwala2021joint,
  author = {Kalyan Vasudev Alwala and Mustafa Mukadam},
  title = {{Joint Sampling and Trajectory Optimization over Graphs for Online Motion Planning}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2021},
  doi = {10.1109/iros51168.2021.9636064},
}
Figure