Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

PWTO: A Heuristic Approach for Trajectory Optimization in Complex Terrains

Yilin Cai, Zhongqiang Ren

TL;DR

PWTO tackles trajectory optimization in complex terrains by first generating a diverse set of Pareto-optimal path seeds on a lattice via MOA*, then refining each seed with Direct Collocation in a round-robin, episodic scheme to explore multiple local minima. The approach yields low-cost, dynamically feasible trajectories and provides anytime solutions that improve as more episodes run, with empirical results showing substantial cost reductions versus baselines and successful Gazebo validations for mobile and quadruped platforms. By leveraging a continuous cost field and a two-stage pipeline, PWTO balances global search with local optimization and demonstrates practical viability in challenging terrains. The authors release open-source code and outline future work toward theoretical guarantees on solution quality.

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.

PWTO: A Heuristic Approach for Trajectory Optimization in Complex Terrains

TL;DR

PWTO tackles trajectory optimization in complex terrains by first generating a diverse set of Pareto-optimal path seeds on a lattice via MOA*, then refining each seed with Direct Collocation in a round-robin, episodic scheme to explore multiple local minima. The approach yields low-cost, dynamically feasible trajectories and provides anytime solutions that improve as more episodes run, with empirical results showing substantial cost reductions versus baselines and successful Gazebo validations for mobile and quadruped platforms. By leveraging a continuous cost field and a two-stage pipeline, PWTO balances global search with local optimization and demonstrates practical viability in challenging terrains. The authors release open-source code and outline future work toward theoretical guarantees on solution quality.

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.
Paper Structure (28 sections, 4 equations, 4 figures, 1 table, 1 algorithm)

This paper contains 28 sections, 4 equations, 4 figures, 1 table, 1 algorithm.

Table of Contents

  1. Introduction
  2. Related Work
  3. Terrain Traversability
  4. Dynamically Feasible Trajectory Planning
  5. Multi-Objective Path Planning (MOPP)
  6. Problem Description
  7. Method
  8. Overview
  9. Part I: Multi-Objective Path Planning
  10. State and Action Spaces
  11. Vector-cost Edges
  12. Non-nominated Paths
  13. MOA* Search
  14. Filtering Paths Using Hausdorff Distance
  15. Part II: Trajectory Optimization
  16. ...and 13 more sections

Figures (4)

  • Figure 1: Overview of proposed Pareto-optimal Warm-started Trajectory Optimization method for robot trajectory planning to navigate complex terrains. (a-b) A continuous traversability cost field and its corresponding terrain map. (c) A set of Pareto-optimal paths obtained from multi-objective graph search in the state lattice. (d) Trajectories reported by PWTO in a round-robin fashion. (e) Visualization of the trajectories for robot to track on rough terrain.
  • Figure 2: Test results of 10 instances in a more complicated cost field with $\sigma=0.0002, M=133$. The starting position is indicated as red circle and the goal is indicated as red star. (a) shows the CR of each baselines with failed instances excluded. (b-f) show detailed information about an instance where all approaches successfully solve. In (b), the horizontal axis shows the number of episodes required by PWTO to find each converged solution trajectory, and the vertical axis shows the cost ratio of each solution over the cheapest trajectory cost found by PWTO. (c-f) show the solution trajectories returned by each approach.
  • Figure 3: Simulation and validation of the PWTO for the trajectory planning of mobile robots. (a) shows all the trajectories (yellow) reported during the PWTO computation, the cheapest trajectory (green) that is used as the reference, and the actual trajectory executed by the robot (red). (b) visualizes the Gazebo simulation. With the closed-loop control, the robot is able to follow the reference trajectory in complex terrains.
  • Figure 4: Simulation and validation of the PWTO for a quadruped robot. (a) shows all the paths (blue) and converged trajectories (red) reported during the PWTO computation. (b) visualizes the Gazebo simulation of a quadruped robot following the reference trajectory in complex terrains.

Theorems & Definitions (2)

  • Definition 1: Dominance
  • Definition 2: Hausdorff Distance