Generalizing Trajectory Retiming to Quadratic Objective Functions

TL;DR

This paper introduces a novel factor graph-based algorithm for trajectory retiming that optimizes quadratic objectives, extending beyond minimum-time solutions to improve robot performance with comparable or faster computation.

Contribution

It presents a new algorithm capable of globally optimizing quadratic objectives in trajectory retiming, extending prior methods limited to boundary solutions with linear time complexity.

Findings

Achieves better real-world robot performance with quadratic objectives.

Maintains linear time complexity similar to existing algorithms.

Comparable or faster than state-of-the-art retiming methods.

Abstract

Trajectory retiming is the task of computing a feasible time parameterization to traverse a path. It is commonly used in the decoupled approach to trajectory optimization whereby a path is first found, then a retiming algorithm computes a speed profile that satisfies kino-dynamic and other constraints. While trajectory retiming is most often formulated with the minimum-time objective (i.e. traverse the path as fast as possible), it is not always the most desirable objective, particularly when we seek to balance multiple objectives or when bang-bang control is unsuitable. In this paper, we present a novel algorithm based on factor graph variable elimination that can solve for the global optimum of the retiming problem with quadratic objectives as well (e.g. minimize control effort or match a nominal speed by minimizing squared error), which may extend to arbitrary objectives with iteration. Our work extends prior works, which find only solutions on the boundary of the feasible region, while maintaining the same linear time complexity from a single forward-backward pass. We experimentally demonstrate that (1) we achieve better real-world robot performance by using quadratic objectives in place of the minimum-time objective, and (2) our implementation is comparable or faster than state-of-the-art retiming algorithms.