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 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 of 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 comparisons with a baseline to showcase the effectiveness of our approach in obtaining smooth, stable trajectories.

In this paper, we solve a joint cooperative localization and path planning problem for a group of Autonomous Aerial Vehicles (AAVs) in GPS-denied areas using nonlinear model predictive control (NMPC). A moving horizon estimator (MHE) is used to estimate the vehicle states with the help of relative bearing information to known landmarks and other vehicles. The goal of the NMPC is to devise optimal paths for each vehicle between a given source and destination while maintaining desired localization accuracy. Estimating localization covariance in the NMPC is computationally intensive; hence, we develop an approximate analytical closed-form expression based on the relationship between covariance and path lengths to landmarks. Using this expression while computing NMPC commands reduces the computational complexity significantly. We present numerical simulations to validate the proposed approach for different numbers of vehicles and landmark configurations. We also compare the results with EKF and RRT*based methods to show the superiority of the proposed closed-form approach.

Unmanned aerial vehicles (UAVs) have reached significant maturity over several years for safe civilian operations like mapping, search and rescue. The operation performance can be significantly improved by deploying multiple cooperating UAVs and optimal decision making. In this work, we present the use of nonlinear model predictive control (NMPC) for two different applications involving cooperative UAVs.