PWTO: A Heuristic Approach for Trajectory Optimization in Complex Terrains

TL;DR

This paper introduces PWTO, a hybrid heuristic method combining graph search and local optimization to generate low-cost, feasible trajectories for robots navigating complex terrains, outperforming baseline methods.

Contribution

The paper presents PWTO, a novel approach that integrates multi-objective graph search with trajectory optimization to effectively handle complex terrain planning problems.

Findings

PWTO achieves solutions with less than half the cost of baseline methods.

PWTO successfully verified in Gazebo simulations with wheeled and quadruped robots.

The approach is open source for community use and further development.

Abstract

This paper considers a trajectory planning problem for a robot navigating complex terrains, which arises in applications ranging from autonomous mining vehicles to planetary rovers. The problem seeks to find a low-cost dynamically feasible trajectory for the robot. The problem is challenging as it requires solving a non-linear optimization problem that often has many local minima due to the complex terrain. To address the challenge, we propose a method called Pareto-optimal Warm-started Trajectory Optimization (PWTO) that attempts to combine the benefits of graph search and trajectory optimization, two very different approaches to planning. PWTO first creates a state lattice using simplified dynamics of the robot and leverages a multi-objective graph search method to obtain a set of paths. Each of the paths is then used to warm-start a local trajectory optimization process, so that different local minima are explored to find a globally low-cost solution. In our tests, the solution cost computed by PWTO is often less than half of the costs computed by the baselines. In addition, we verify the trajectories generated by PWTO in Gazebo simulation in complex terrains with both wheeled and quadruped robots. The code of this paper is open sourced and can be found at https://github.com/rap-lab-org/public_pwto.