New Pareto-optimal motion planning algorithms for kinodynamic robots

Researchers Yusif Razzaq, Anne Theurkauf, Nisar Ahmed, and Morteza Lahijanian have tackled the complex problem of multi-objective motion planning under kinodynamic constraints. In their new paper, they demonstrate that traditional cost scalarization methods fail for continuous-domain systems. To overcome this, they extend the Stable Sparse-RRT (SST) algorithm by maintaining a representative set of locally Pareto-optimal nodes at each witness neighborhood, rather than a single node. This yields three distinct algorithms: lexSST for lexicographic optimization (objectives minimized in strict priority order), coSST for constrained optimization (minimizing a primary objective within bounds on others), and poSST for approximating the full Pareto-optimal trade-off front.

The authors provide formal theoretical guarantees—probabilistic completeness and asymptotic optimality—for all three algorithms. Extensive empirical evaluations on simulated robotic systems confirm that the algorithms effectively discover optimal trajectories across competing objectives like time, energy, and safety. This work bridges a gap between theoretical multi-objective optimization and practical motion planning, offering a unified framework that can be deployed in autonomous vehicles, drones, and manipulators where multiple conflicting goals must be balanced in real time.