iDb-A*: Iterative Search and Optimization for Optimal Kinodynamic Motion Planning

Joaquim Ortiz-Haro, Wolfgang Honig, Valentin Hartmann, and Marc Toussaint

Back to personal webpage: here

TL;DR

Iterative Discontinuity Bounded A* (iDb-A*) is a new kinodynamic motion planner that combines search and optimization in an iterative way. It is asymptotically optimal and faster than state-of-the-art motion planners in a wide range of problems with car-like and flying robots.

Paper

Journal Version – Preprint submitted to IEEE Transactions on Robotics (T-RO) in November 2023 (Recommended)

Conference Paper (IROS 2022)

Code

My Happy SVG Dynoplan :t-rex:

My Happy SVG Dynobench :t-rex:

Supplementary Video

Abstract

Motion planning for robotic systems with complex dynamics is a challenging problem. While recent sample-based algorithms achieve asymptotic optimality by propagating random control inputs, their empirical convergence rate is poor, specially in high-dimensional systems such as multirotors. An alternative approach is to first plan with a simplified geometric model and then use trajectory optimization to follow the geometric path while accounting for the true dynamics. This approach is incomplete, often failing to produce valid motion plans, and requires in-depth knowledge of each dynamical system.

In this paper, we present Iterative Discontinuity Bounded A* (iDb-A*), a new kinodynamic motion planner that combines search and optimization in an iterative way. The search step uses a finite set of short trajectories (motion primitives) that are connected while allowing a bounded discontinuity, and the optimization step locally repairs the discontinuities with trajectory optimization. By iteratively reducing the allowed discontinuity and adding more motion primitives, our algorithm achieves asymptotic optimality with a very good any-time behaviour. We provide a benchmark of 43 problems with 8 different dynamical systems, including different versions of unicycles and multirotors. Compared to the state-of-the-art, iDb-A* consistently solves more problem instances and finds lower-cost solutions faster.

Dynobench :t-rex:

Dynobench 🦖 is a universal benchmark for kinodynamic motion planning. Develop, combine and compare different methods, from trajectory optimization and sample based motion planning to supervised and reinforcement learning. We currently support 8 dynamical Systems, and a total of 43 problems! (more to come!)

Dynoplan :t-rex:

Dynoplan is a small library for solving kinodynamic motion planning problems, as defined in Dynobench :t-rex:. It implements 3 different algorithms: Trajectory Optimization with geometric initial guess , Sample based Motion Planning, and Iterative Search and Optimization (iDb-A*).

Motion Primitives

All the motion primitives are available in Github and Google Drive (check instructions in Dynobench)

Extended results

We compare iDb-A* against SST* (sample-based optimal kynodynamic motion planner) and RRT* -TO (Combination of Asymptotically Optimal Geometric Planner with Trajectory Optimization). Check our paper for full results and discussion. These results can be reproduced using our code Dynoplan

Benchmark

Trajectory Optimization with Free Terminal Time

Heuristic Functions

Analysis of Underlying Optimization Algorithm (Single vs Multiple Shooting)

We repeat the benchmark using a multiple shooting formulation and Sequential Quadratic Programming for the trajectory optimization step of iDb-A* (instead of Differential Dynamic Programming). The suffix “-m” indicates that we use a multiple shooting solver instead of Differential Dynamic Programming. This benchmark was run on an Intel i9-14900KF CPU.

Link to PDF

Convergence Plots for all Problems

We provide the convergence plots for all problems, which display the median of the cost and success rate over 20 runs, with a 95% confidence interval, which provides a clear view of the variance of the different algorithms. This results correspond to the table shown above (Intel i9-14900KF CPU).

Link to PDF.