  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.
