Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Bi-level Trajectory Optimization on Uneven Terrains with Differentiable Wheel-Terrain Interaction Model

Amith Manoharan, Aditya Sharma, Himani Belsare, Kaustab Pal, K. Madhava Krishna, Arun Kumar Singh

TL;DR

This paper expresses the wheel-terrain interaction and 6dof pose prediction as a non-linear least squares (NLS) problem and shows that the NLS-based pose prediction closely matches the output of a high-fidelity physics engine, making the pose predictor a differentiable wheel-terrain interaction model.

Abstract

Navigation of wheeled vehicles on uneven terrain necessitates going beyond the 2D approaches for trajectory planning. Specifically, it is essential to incorporate the full 6dof variation of vehicle pose and its associated stability cost in the planning process. To this end, most recent works aim to learn a neural network model to predict the vehicle evolution. However, such approaches are data-intensive and fraught with generalization issues. In this paper, we present a purely model-based approach that just requires the digital elevation information of the terrain. Specifically, we express the wheel-terrain interaction and 6dof pose prediction as a non-linear least squares (NLS) problem. As a result, trajectory planning can be viewed as a bi-level optimization. The inner optimization layer predicts the pose on the terrain along a given trajectory, while the outer layer deforms the trajectory itself to reduce the stability and kinematic costs of the pose. We improve the state-of-the-art in the following respects. First, we show that our NLS based pose prediction closely matches the output from a high-fidelity physics engine. This result coupled with the fact that we can query gradients of the NLS solver, makes our pose predictor, a differentiable wheel-terrain interaction model. We further leverage this differentiability to efficiently solve the proposed bi-level trajectory optimization problem. Finally, we perform extensive experiments, and comparison with a baseline to showcase the effectiveness of our approach in obtaining smooth, stable trajectories.

Bi-level Trajectory Optimization on Uneven Terrains with Differentiable Wheel-Terrain Interaction Model

TL;DR

This paper expresses the wheel-terrain interaction and 6dof pose prediction as a non-linear least squares (NLS) problem and shows that the NLS-based pose prediction closely matches the output of a high-fidelity physics engine, making the pose predictor a differentiable wheel-terrain interaction model.

Abstract

Navigation of wheeled vehicles on uneven terrain necessitates going beyond the 2D approaches for trajectory planning. Specifically, it is essential to incorporate the full 6dof variation of vehicle pose and its associated stability cost in the planning process. To this end, most recent works aim to learn a neural network model to predict the vehicle evolution. However, such approaches are data-intensive and fraught with generalization issues. In this paper, we present a purely model-based approach that just requires the digital elevation information of the terrain. Specifically, we express the wheel-terrain interaction and 6dof pose prediction as a non-linear least squares (NLS) problem. As a result, trajectory planning can be viewed as a bi-level optimization. The inner optimization layer predicts the pose on the terrain along a given trajectory, while the outer layer deforms the trajectory itself to reduce the stability and kinematic costs of the pose. We improve the state-of-the-art in the following respects. First, we show that our NLS based pose prediction closely matches the output from a high-fidelity physics engine. This result coupled with the fact that we can query gradients of the NLS solver, makes our pose predictor, a differentiable wheel-terrain interaction model. We further leverage this differentiability to efficiently solve the proposed bi-level trajectory optimization problem. Finally, we perform extensive experiments, and comparison with a baseline to showcase the effectiveness of our approach in obtaining smooth, stable trajectories.
Paper Structure (17 sections, 1 theorem, 31 equations, 7 figures)

This paper contains 17 sections, 1 theorem, 31 equations, 7 figures.

Table of Contents

  1. Introduction
  2. Related Works
  3. Preliminaries
  4. Primer on Chain Rule of Differentiation
  5. Trajectory Parametrization
  6. Tip-over stability criteria
  7. Main Algorithmic Results
  8. Functional Form for the Terrain Model
  9. Differentiable Wheel-Terrain Interaction
  10. Bi-Level Optimization Based Trajectory Planning
  11. Projected gradient descent
  12. Results
  13. Implementation Details
  14. Validation using the Gazebo simulator
  15. Validation of the Safe Planning on Uneven Terrain
  16. ...and 2 more sections

Key Result

Proposition 1

Consider the NLS problem eqn:u_k. We can define $\mathbf{H}=\mathrm{D}_{\mathbf{u}_k\mathbf{u}_k}^2 \mathbf{g}(\mathbf{x}_k,\mathbf{u}_k(\mathbf{x}_k)) \in \mathbb{R}^{m \times m}$, where $\mathbf{g(.)}$ is obtained by stacking $g_j(.)$ and $\mathbf{B}=\mathrm{D}_{\mathbf{x_ku_k}}^2\mathbf{g}(\mathb with the assumption that $\mathbf{H}$ is non-singular.

Figures (7)

  • Figure 2: Husky navigating an uneven terrain. The black points represent the entire terrain, whereas the coloured points represent the terrain's local patch. All points within a given radius from the Husky are used to model the terrain, which is used to compute a stable trajectory to the goal. As seen above, the trajectory (shown in pink) successfully avoids the ditch and goes around it to reach the goal.
  • Figure 3: (a) A four-wheeled vehicle with the geometry vectors describing the holonomic constraints. (b) Definition of the vectors associated with tip-over stability.
  • Figure 4: Red points represent the ground truth terrain patch, and green points represent the terrain patch with predicted z coordinates.
  • Figure 5: (a) Trajectory obtained from manually driving Husky on a synthetic terrain in Gazebo. (b) $z$ ground truth and predicted values. (c) $\beta$ and $\gamma$ ground truth vs predicted. (d) Error statistics for $z$. (e) Error statistics for $\beta$ and $\gamma$.
  • Figure 6: Trajectories with and without stability cost. Black colour represents trajectory with stability cost and pink colour without the stability cost.
  • ...and 2 more figures

Theorems & Definitions (3)

  • Remark 1
  • Proposition 1
  • proof