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
Dynoplan 
Dynobench
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
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
Dynoplan is a small library for solving kinodynamic motion planning problems, as defined in Dynobench . 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.
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).