Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

PINSAT: Parallelized Interleaving of Graph Search and Trajectory Optimization for Kinodynamic Motion Planning

Ramkumar Natarajan, Shohin Mukherjee, Howie Choset, Maxim Likhachev

Abstract

Trajectory optimization is a widely used technique in robot motion planning for letting the dynamics and constraints on the system shape and synthesize complex behaviors. Several previous works have shown its benefits in high-dimensional continuous state spaces and under differential constraints. However, long time horizons and planning around obstacles in non-convex spaces pose challenges in guaranteeing convergence or finding optimal solutions. As a result, discrete graph search planners and sampling-based planers are preferred when facing obstacle-cluttered environments. A recently developed algorithm called INSAT effectively combines graph search in the low-dimensional subspace and trajectory optimization in the full-dimensional space for global kinodynamic planning over long horizons. Although INSAT successfully reasoned about and solved complex planning problems, the numerous expensive calls to an optimizer resulted in large planning times, thereby limiting its practical use. Inspired by the recent work on edge-based parallel graph search, we present PINSAT, which introduces systematic parallelization in INSAT to achieve lower planning times and higher success rates, while maintaining significantly lower costs over relevant baselines. We demonstrate PINSAT by evaluating it on 6 DoF kinodynamic manipulation planning with obstacles.

PINSAT: Parallelized Interleaving of Graph Search and Trajectory Optimization for Kinodynamic Motion Planning

Abstract

Trajectory optimization is a widely used technique in robot motion planning for letting the dynamics and constraints on the system shape and synthesize complex behaviors. Several previous works have shown its benefits in high-dimensional continuous state spaces and under differential constraints. However, long time horizons and planning around obstacles in non-convex spaces pose challenges in guaranteeing convergence or finding optimal solutions. As a result, discrete graph search planners and sampling-based planers are preferred when facing obstacle-cluttered environments. A recently developed algorithm called INSAT effectively combines graph search in the low-dimensional subspace and trajectory optimization in the full-dimensional space for global kinodynamic planning over long horizons. Although INSAT successfully reasoned about and solved complex planning problems, the numerous expensive calls to an optimizer resulted in large planning times, thereby limiting its practical use. Inspired by the recent work on edge-based parallel graph search, we present PINSAT, which introduces systematic parallelization in INSAT to achieve lower planning times and higher success rates, while maintaining significantly lower costs over relevant baselines. We demonstrate PINSAT by evaluating it on 6 DoF kinodynamic manipulation planning with obstacles.
Paper Structure (19 sections, 3 theorems, 15 equations, 3 figures, 1 table, 3 algorithms)

This paper contains 19 sections, 3 theorems, 15 equations, 3 figures, 1 table, 3 algorithms.

Table of Contents

  1. Introduction
  2. Related Work
  3. Kinodynamic Motion Planning
  4. Parallel Search
  5. Problem Formulation
  6. Background
  7. B-Splines
  8. Approach
  9. Low Dimensional Graph Search
  10. w-eA*
  11. B-Spline Optimization
  12. Decoupling Optimization over Trajectory and its Duration
  13. Transcription for Optimizing B-splines
  14. B-spline Trajectory Reconstruction
  15. PINSAT: Parallelized Interleaving of Search And Trajectory Optimization
  16. ...and 4 more sections

Key Result

Lemma 1

If $\tau(e)$ is convex, then a $\boldsymbol{\phi}_{\mathbf{x}\xspace^\prime\mathbf{x}\xspace^{\prime\prime}}^\sim(t) \in \tau(e), \forall t\in[0,t_{\text{max}}]$ that solves Eq. eq:tobj to global optimality can be found.

Figures (3)

  • Figure 1: ABB arm evading obstacles to block a ball thrown at it using kinodynamic motion produced by PINSAT.
  • Figure 2: Graphical illustration of PINSAT. (Fig. a, b) The dummy edge $e^d_0$ from $\mathbf{x}\xspace^0$ is expanded to get real low-D edges $[e^1_0, e^2_0, e^3_0]$ but not yet evaluated with the optimizer in full-D to generate the state successor (denoted with hollow node). (c) The ePA*SE architecture in PINSAT then evaluates multiple edges in parallel (red curves). Once evaluated the outgoing nodes $\mathbf{x}\xspace^1\&\mathbf{x}\xspace^3$ are represented with dummy edges $e^d_1$ & $e^d_3$ and inserted in OPEN. This node is shown with a gray gradient to represent the underspecified full-D state for the optimization to figure out. In Fig. (d), we highlight the asynchronous execution of PINSAT. Here the dummy edges $e^d_1$ & $e^d_3$ are expanded into real edges and the $e^2_0$ is lifted to full-D by running the optimizer. Similarly, in Fig. (e), more edges are concurrently lifted to full-D. Fig. (f) denotes how the short incremental trajectory generated in Fig. (e) is reused for warm-starting the optimization and generating the trajectories from $\mathbf{x}\xspace^S$. This is a powerful step that dramatically increases the speed of convergence. Note that once the trajectory from $\mathbf{x}\xspace^S$ is generated, it replaces the low-D edge and helps drive the graph search into more informative directions.
  • Figure 3: PINSAT used to accelerate preprocessing of a motion library for rapid lookup. Here, an ABB arm with a shield attached to its end-effector executes a motion generated by PINSAT to block a ball launched toward it.

Theorems & Definitions (10)

  • Remark 1
  • Remark 2
  • Remark 3
  • Definition 1
  • Remark 4
  • Lemma 1
  • proof
  • Lemma 2
  • proof
  • Theorem 1