TRON: A Fast Solver for Trajectory Optimization with Non-Smooth Cost Functions.

Anirudh Vemula, J. Andrew BagnellarXiv 2020 · OpenReview

Abstract

Trajectory optimization is an important tool for control and planning of complex, underactuated robots, and has shown impressive results in real world robotic tasks. However, in applications where the cost function to be optimized is non-smooth, modern trajectory optimization methods have extremely slow convergence. In this work, we present TRON, an iterative solver that can be used for efficient trajectory optimization in applications with non-smooth cost functions that are composed of smooth components. TRON achieves this by exploiting the structure of the objective to adaptively smooth the cost function, resulting in a sequence of objectives that can be efficiently optimized. TRON is provably guaranteed to converge to the global optimum of the non-smooth convex cost function when the dynamics are linear, and to a stationary point when the dynamics are nonlinear. Empirically, we show that TRON has faster convergence and lower final costs when compared to other trajectory optimization methods on a range of simulated tasks including collision-free motion planning for a mobile robot, sparse optimal control for surgical needle, and a satellite rendezvous problem.

Introduction

Trajectory optimization is a fundamental component of robot motion planning, enabling complex systems to find feasible paths while minimizing energy, time, or other costs. Most popular solvers, such as the Iterative Linear Quadratic Regulator (ILQR), rely on the assumption that cost functions are smooth and differentiable. However, many real-world robotic applications involve inherently non-smooth costs. For example, obstacle avoidance often uses "max" operators to penalize proximity to walls, and sparse control (using $L_1$-norms) is used to ensure thrusters or steering are used only when necessary. When encountering these "kinks" in the cost landscape, standard solvers often suffer from extremely slow convergence or fail to find optimal solutions. TRON addresses this by exploiting the specific structure of these non-smooth costs rather than treating them as black boxes.

Methodology

TRON (Trajectory Optimization with Non-smooth costs) handles objectives composed of smooth components, such as $f(x) + \max(g_1(x), g_2(x))$. The solver's core innovation is a two-step approach:

  1. Min-Max Formulation: The non-smooth "max" term is reformulated as a linear combination of its components using a dual variable ($\theta$) that lives on a simplex. This transforms the optimization into a two-player game where one player minimizes the cost and the other maximizes the weight on the most expensive component.
  2. Adaptive Smoothing via KL-Divergence: To stabilize the optimization, TRON adds a Kullback-Leibler (KL) divergence penalty. This regularizes the dual variable relative to its previous value, effectively smoothing the objective. By using a decreasing penalty coefficient, TRON starts with a smooth approximation to make rapid progress and gradually "sharpens" the objective to converge to the exact non-smooth optimum.

This formulation allows for closed-form updates similar to exponentiated gradient descent, making TRON easy to integrate with existing smooth solvers like ILQR or Differential Dynamic Programming (DDP).

Experimental Evaluation

The performance of TRON was evaluated against standard baselines like ILQR and ADMM across four simulated tasks: a 2D Lasso problem, collision-free mobile robot navigation, surgical steerable needle control, and satellite rendezvous. These tasks test the solver's ability to handle both sparsity and complex obstacle avoidance.

TRON vs ILQR on a collision-free mobile robot task. TRON finds a lower-cost path significantly faster.

In the mobile robot task, TRON successfully navigated a crowded environment with 11 obstacles where standard ILQR failed to converge to a collision-free path. The solver demonstrated superior reliability in finding feasible trajectories in constrained spaces.

Results and Discussion

TRON consistently demonstrated faster convergence and lower final costs compared to ILQR and ADMM. A significant highlight was its performance in sparse control tasks, such as the satellite rendezvous problem.

L1-norm of control input and trajectory cost for the satellite rendezvous problem. TRON achieves better sparsity and faster convergence than ILQR and ADMM.

As shown in the satellite rendezvous results, TRON achieved the desired "bang-off-bang" control behavior—where thrusters are either fully on or completely off—much more effectively than ADMM, which often failed to reach true zero-valued controls. Furthermore, TRON reached low-cost solutions in a fraction of the time required by other methods. This efficiency stems from TRON's ability to exploit the problem structure rather than relying on general-purpose non-smooth optimization techniques.

Conclusion

TRON provides a robust and efficient framework for trajectory optimization with non-smooth costs. By combining min-max game theory with information-theoretic smoothing, it overcomes the convergence issues that plague traditional solvers in the presence of cost "kinks." Future work includes extending TRON to handle a larger number of functions within the "max" operator and integrating Augmented Lagrangian techniques to support more complex non-linear constraints.

Summary updated

Abstract

Trajectory optimization is an important tool for control and planning of complex, underactuated robots, and has shown impressive results in real world robotic tasks. However, in applications where the cost function to be optimized is non-smooth, modern trajectory optimization methods have extremely slow convergence. In this work, we present TRON, an iterative solver that can be used for efficient trajectory optimization in applications with non-smooth cost functions that are composed of smooth components. TRON achieves this by exploiting the structure of the objective to adaptively smooth the cost function, resulting in a sequence of objectives that can be efficiently optimized. TRON is provably guaranteed to converge to the global optimum of the non-smooth convex cost function when the dynamics are linear, and to a stationary point when the dynamics are nonlinear. Empirically, we show that TRON has faster convergence and lower final costs when compared to other trajectory optimization methods on a range of simulated tasks including collision-free motion planning for a mobile robot, sparse optimal control for surgical needle, and a satellite rendezvous problem.
BibTeX
@misc{vemula2020tron,
  author = {Anirudh Vemula and J. Andrew Bagnell},
  title = {{TRON: A Fast Solver for Trajectory Optimization with Non-Smooth Cost Functions.}},
  year = {2020},
  eprint = {2003.14393},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2003.14393},
}
Figure