A Convex Optimization Approach to Smooth Trajectories for Motion Planning with Car-Like Robots

TL;DR

This paper introduces a convex optimization-based heuristic algorithm called CES for smoothing and optimizing trajectories of car-like robots, achieving high-quality results rapidly suitable for real-time applications.

Contribution

The paper presents the CES algorithm, a novel convex programming approach for fast trajectory smoothing and speed optimization for car-like robots, improving upon jagged outputs of sampling-based methods.

Findings

CES produces smooth, collision-free trajectories in hundreds of milliseconds

The algorithm is suitable for real-time motion planning

Numerical experiments demonstrate high-quality solutions

Abstract

In the recent past, several sampling-based algorithms have been proposed to compute trajectories that are collision-free and dynamically-feasible. However, the outputs of such algorithms are notoriously jagged. In this paper, by focusing on robots with car-like dynamics, we present a fast and simple heuristic algorithm, named Convex Elastic Smoothing (CES) algorithm, for trajectory smoothing and speed optimization. The CES algorithm is inspired by earlier work on elastic band planning and iteratively performs shape and speed optimization. The key feature of the algorithm is that both optimization problems can be solved via convex programming, making CES particularly fast. A range of numerical experiments show that the CES algorithm returns high-quality solutions in a matter of a few hundreds of milliseconds and hence appears amenable to a real-time implementation.