# A Safety-Critical Framework for UGVs in Complex Environments:
A Data-Driven Discrepancy-Aware Approach

Skylar X. Wei  Lu Gan  Joel W. Burdick

###### Abstract

This work presents a novel data-driven multi-layered planning and control
framework for the safe navigation of a class of unmanned ground vehicles
(UGVs) in the presence of unknown stationary obstacles and additive modeling
uncertainties. The foundation of this framework is a novel robust model
predictive planner, designed to generate optimal collision-free trajectories
given an occupancy grid map, and a paired ancillary controller, augmented to
provide robustness against model uncertainties extracted from learning data.

To tackle modeling discrepancies, we identify both matched (input
discrepancies) and unmatched model residuals between the true and the nominal
reduced-order models using closed-loop tracking errors as training data.
Utilizing conformal prediction, we extract probabilistic upper bounds for the
unknown model residuals, which serve to construct a robustifying ancillary
controller. Further, we also determine maximum tracking discrepancies, also
known as the robust control invariance tube, under the augmented policy,
formulating them as collision buffers. Employing a LiDAR-based occupancy map
to characterize the environment, we construct a discrepancy-aware cost map
that incorporates these collision buffers. This map is then integrated into a
sampling-based model predictive path planner that generates optimal and safe
trajectories that can be robustly tracked by the augmented ancillary
controller in the presence of model mismatches.

The effectiveness of the framework is experimentally validated for autonomous
high-speed trajectory tracking in a cluttered environment with four different
vehicle-terrain configurations. We also showcase the framework’s versatility
by reformulating it as a driver-assist program, providing collision avoidance
corrections based on user joystick commands.

###### keywords:

Robust Planning , Safety-Critical Planning , Conformal Prediction , Risk-Aware
Control , Model Predictive Path Integral , Uncertainty Quantification

††journal: Robotics and Autonomous Systems

[label1]organization=Division of Engineering and Applied Sciences,California
Institute of Technology,addressline=1200 E. California Blvd., city=Pasadena,
postcode=91125, state=CA, country=USA

[label2]organization=The School of Aerospace Engineering, Georgia Institute of
Technology,addressline=North Avenue, city=Atlanta, postcode=30332, state=GA,
country=USA

##  1 Introduction

Figure 1: (Top) An overview of our three-step procedure, summarizes the
safety-critical framework: (1) offline data-driven model discrepancy
identification and learning, (2) augmentation of the control policy based on
the learned upper bounds and the associated collision buffers, and (3) a
discrepancy-aware MPPI algorithm provides receding-horizon safe trajectory and
input pairs. (Bottom) snapshots of the UGV movement in high-speed trajectory
tracking in a cluttered environment, given unsafe desired trajectories.

Safety is a critical requirement for autonomous exploration of complex
environments by unmanned ground vehicles (UGVs) which have a wide range of
applications such as agriculture [1], search and rescue [2], package delivery
[3], and mining [4]. Specifically, providing collision avoidance guarantees
when navigating in unknown and unstructured environments poses unique
challenges, such as model mismatches induced by the complex environments [5,
6] and incomplete obstacle descriptions [7, 8] in the case of a priori unknown
environment maps. Systematic and provable probabilistic frameworks for UGV
traversal are needed to provide safety guarantees in such complex scenarios.

Despite the increasing complexity of UGVs and their deployment in diverse
terrains, reduced-order models are commonly employed for vehicle planning and
control design [9, 10, 11]. However, a planning and control framework that
relies on these models must be robustified against disturbances (e.g. to
account for the residual difference between the reduced-order and true models)
to guarantee safe and reliable traversal. This paper introduces a data-driven
technique to provide these guarantees even in the presence of a simplified
(still underactuated) vehicle model with matched and unmatched model
uncertainties [12, 13, 14].

Our method augments the vehicle’s controller using learned dynamic
discrepancies and applies rigorous uncertainty quantification to provide
provably safe operation. Example model discrepancies include wheel slipping
and skidding, ignored actuation dynamics, and communication delays.

The proposed framework automatically learns an upper bound on model residuals
from data and systematically calculates the corresponding collision buffers
needed to provide guaranteed probabilistic safe navigation of UGV systems in
unknown and cluttered environments. This feature largely eliminates the “hand
tuning” of the underlying planner and controller that is normally required for
simplified models.

Our framework consists of the following contributions (see Fig. 1):

  1. 1.

Data-Driven Discrepancy Identification: Using conformal prediction [15], we
extract a probabilistic upper bound of matched (controllable input)
discrepancies and unmatched discrepancies from training data, without any
assumptions on the discrepancy distributions.

  2. 2.

Controller Augmentation: The identified upper bounds are then used to augment
the vehicle’s nominal ancillary controller to ensure closed-loop
stabilizability and robustness against uncertainties.

  3. 3.

Collision Buffer Construction: To ensure safety given model uncertainties, we
deduce a maximum trajectory tracking deviation of the closed-loop system under
the augmented controller. This collision buffer delineates the boundaries
within which the system can operate safely, despite mismatches between the
nominal and true vehicle models.

  4. 4.

Discrepancy-Aware Planner: A discrepancy-aware cost map is constructed from
the identified collision buffer and a sensory-derived occupancy map. This cost
map can be seamlessly used with Model Predictive Path Integral (MPPI) to
generate optimal finite-horizon trajectories that provably adhere to a user-
chosen risk tolerance.

The effectiveness of the proposed framework is demonstrated via hardware
experiments on a differential-driven ground vehicle under four different
vehicle and terrain configurations. Additionally, we adapted the proposed
framework to provide collision avoidance for a human-in-the-loop driver
assistance application.

###  1.1 Related Work

Prior research has studied the impact of unmodeled disturbances on vehicle
planning and control using frameworks such as input-to-state safe control [16,
17] and risk-aware control [18, 19]. However, these methods often assume a
priori model knowledge or require a minimum understanding of the disturbances’
magnitudes or distributions. Bayesian Optimization and Reinforcement Learning
methods can bypass the uncertainty or model identification steps, directly
learning risk-aware control policies in a model-free fashion [20, 21]. These
approaches still come with the assumption of a priori knowledge of the
disturbances, or that they can be sampled. More recently, the union of Neural
Networks with adaptive control [22] has demonstrated remarkable tracking
improvements in drone control given unknown residual dynamics. However, the
theoretical robustness guarantees in [22] rely on knowledge of wind
disturbance upper bounds, and they do not consider obstacles.

A learned control policy can be combined with an optimal path planner, as
demonstrated in robust model predictive control [23, 24] and chance-
constrained stochastic optimal control [25, 26]. Although these methods
construct a deterministic problem surrogate to the original probabilistic one,
they often require constraint convexification, such as sequential constraint
linearization around fixed points, and optimality can only be reached in
infinite sequential iterations. For instance, Monte Carlo Sample propagation
[27, 28] and scenario-based approaches [29] can require a very large number of
samples to reach the desired obstacle avoidance probability guarantees.

The sampling-based Model Predictive Path Integral control (MPPI) method has
proven versatility in off-road racing applications [11]. As a model-based
strategy, MPPI may exhibit suboptimal performance in the face of modeling
uncertainties and disturbances. Tube-based [10] and robust MPPI variants [30]
have been proposed to address these challenges. Both of these methods
integrate an ancillary controller, such as iterative Linear Quadratic Gaussian
(iLQG), to improve tracking performance with measurement feedback and
robustness to uncertainties. These methods’ assumptions on sequential
linearization and the additive process and measurement noise being Gaussian
limits their applicability to nonlinear systems with generalized additive
model uncertainties.

In unknown environments, UGV navigation often relies on sensor-based occupancy
grid maps [31, 32], which are commonly used for global path planning [33].
Grid maps can be used for local optimization-based path planning by converting
the maps into signed distance functions [34, 35], which can be locally
convexified to serve as the collision-avoiding state constraints.
Alternatively, occupancy-based risk maps have been used for sampling-based
local planners [36, 37, 38, 7]. To alleviate the computation and convexity
burden, the MPPI algorithm allows direct grid-map-based assessment for
trajectory costs [38]. However, systematic parameter and cost tuning is
required to avoid the algorithm making undesirable decisions [39, 40]. Lastly,
direct estimation of traversability from sensors is also a popular technique
for robotic navigation in complex environments leveraging expert heuristics
[7, 9] or self-supervised learning [41]. However, it is not straightforward to
extend these hardware successes to provide provable safety guarantees.

###  1.2 Notation and Organization

The set of integers, positive integers, natural numbers, real numbers,
positive reals, and non-negative reals are denoted as
$\mathbb{Z},\mathbb{Z}_{>0}$, $\mathbb{N}$, $\mathbb{R}$, $\mathbb{R}_{>0}$,
and $\mathbb{R}_{\geq 0}$, respectively. A sequence of consecutive integers,
such as $\\{i,\cdots,i+k\\}$, is denoted as $\mathbb{Z}_{i}^{i+k}$. A finite
sequence of indexed scalars, $\\{a_{1},\cdots,a_{k}\\}$, is represented as
$\\{a_{i}\\}_{i=1}^{k}$ and for vectors of $\mathbf{a}$ as
$\\{\mathbf{a}_{i}\\}_{i=1}^{k}$. Indicator functions are defined as
$\mathbf{1}_{b(a)}:\mathbb{R}\to\\{0,1\\}$, where $b(a)$ is a Boolean
proposition over argument $a$. If $b(a)$ is true, the indicator function
returns $1$. Else it returns $0$.

Section 2 describes the UGV traversal problem, while Section 3 provides the
mathematical preliminaries. The characterization of tracking discrepancies
using conformal prediction is discussed in Section 4, while Section 5 augments
the nominal ancillary controller to account for the model discrepancies, and
derives theoretical bounds on the maximum tracking deviation due to the
discrepancies. Section 6 outlines the construction of a discrepancy-aware cost
map and the discrepancy-aware MPPI planner. Experimental results, discussions,
and future works can be found in Sections 7 and 8.

##  2 Problem Statement

This work investigates the traversal of autonomous ground vehicles, such as
differential drive robots, tracked vehicles, and skid steer vehicles. We adopt
a widely used model of such vehicles for controller design and motion
planning. This kinematic model, e.g. [42, 43], is described as:

$\displaystyle\dot{\bm{x}}=\begin{bmatrix}\cos(\theta)&\sin(\theta)&0\\\
0&0&1\end{bmatrix}^{T}\begin{bmatrix}v\\\
\omega\end{bmatrix}=g(\bm{x})\bm{u}=f(\bm{x},\bm{u}),$ (1)

where the system state
$\bm{x}=[x,y,\theta]^{T}\in\mathcal{D}^{\bm{x}}\subseteq\mathbb{R}^{3}$
consists of the robot’s inertial $x,y$ position and its heading angle
$\theta$. The control inputs
$\bm{u}=[v,\omega]^{T}\in\mathcal{D}^{\bm{u}}\subseteq\mathbb{R}^{2}$ are the
vehicle’s linear and angular velocity in the body $x$\- and $z$-axes,
respectively. The allowable sets of states and inputs are denoted by
$\mathcal{D}^{\bm{x}}$ and $\mathcal{D}^{\bm{u}}$.

The nominal model (1) is a reduced-order description of a general
differential-driven ground vehicle: it assumes that the wheels or tracks
contact the ground at all times, ignoring phenomena such as slipping,
skidding, motor dynamics, and communication delays. A more comprehensive
model, accounting for these neglected factors, can be formulated as:

$\dot{\bm{x}}=\hat{f}(X)=f(\bm{x},\bm{u})+\tilde{f}(\bm{x},\bm{h},\bm{u},\bm{d}(t)),$
(2)

where the true dynamics $\hat{f}:Q\to\mathbb{R}^{3}$ maps state space $Q$ to
the vehicle’s true linear and angular velocities, $f(\bm{x},\bm{u})$ is the
nominal model (1), and $\tilde{f}:Q\to\mathbb{R}^{3}$ is an a priori unknown
additive disturbance, that models the discrepancies–it is possibly a function
of state $\bm{x}$, input $\bm{u}$, process noise $\bm{d}(t)$, and hidden
states $\bm{h}$. The variable $X\in Q$ in equation (2) can be interpreted as a
vehicle operating state that includes inputs that correspond to the vehicle
velocity $\dot{\bm{x}}$ via function $\hat{f}$. To note, we define projections
from the vehicle operating state space $\mathcal{Q}$ to the nominal state
space $\mathcal{D}^{\bm{x}}$ and nominal system input space
$\mathcal{D}^{\bm{u}}$ for analyzing the discrepancy between the not fully
observable true dynamics (2) and the nominal model(1).

######  Definition 1.

Let $\bm{x}_{X}$ be the projection of sample $X$ onto $\mathcal{D}^{\bm{x}}$,
denoted as $P_{X\to\bm{x}}$, is defined as

$P_{X\to\bm{x}}(X)\triangleq\bm{x}_{X}\in\mathcal{D}^{\bm{x}}.$ (3)

where $\mathcal{D}^{\bm{x}}$ is the nominal (reduced-order) admissible states
and $X\in Q$ is a sampled vehicle true state. Let
$\bm{u}_{X}\in\mathcal{D}^{\bm{u}}$ be the projection $P_{X\to\bm{u}}$ of
sample $X$ onto the nominal admissible input space $\mathcal{D}^{\bm{u}}$.

Most importantly, we will only assume the existence projections
$P_{X\to\bm{x}}$ and $P_{X\to\bm{u}}$, and we do not assume these projections
are unique nor knowledge of their analytic forms, etc. Now, the true dynamics
(2) given operating state $X$ satisfies

$\dot{\bm{x}}_{X}=\hat{f}(X)=f(\bm{x}_{X},\bm{u}_{X})+\tilde{f}(X).$ (4)

For notation simplicity, we will drop the subscripts $X$ for $\bm{x}_{X}$ and
$\bm{u}_{X}$ as we refer to the nominal state given any operating sample state
$X$.

Following many robust optimal control approaches [24], we study additive model
uncertainties that satisfy the following assumption.

######  Assumption 1.

The true system dynamics $\hat{f}$ can be expressed as $\hat{f}=f+\tilde{f}$
where $f$ is the nominal dynamics and $\tilde{f}$ represents all model
uncertainties.

Figure 2: (Left) Definition of the polar coordinate. Subscripts $(\cdot)_{b}$,
$(\cdot)_{e}$ denote the body frame and inertial frame, respectively. The pose
with superscript $(\cdot)^{*}$ is the desired pose. (Right) Four
experimentally validated scenarios. Config. A: UGV with flipper on rubber
flooring. Config. B: UGV with flipper on foam flooring. Config. C: UGV without
flipper on rubber flooring. Config. D: UGV without flipper foam flooring.

Our approach is based on the notion that the vehicle aims to follow a desired
(denoted by $(\cdot)^{d}$) reference path $\bm{p}^{d}(t)=[x^{d}(t)$,
$y^{d}(t)]^{T}$, which is designed without considering obstacles or model
simplifications and mismatches. Since the nominal model (1) is differentially
flat, the input sequence given the desired reference
$\bm{p}^{d}(t)=[x^{d}(t),y^{d}(t)]^{T}$ can be readily found using (5),
without considering obstacles. We denote the differentially flat input
sequence given a position reference $\bm{p}^{d}(t)$ as the desired flatness-
based input reference $\bm{u}^{d}=[v^{d},\omega^{d}]$, which can be computed
as

$\displaystyle
v^{d}\\!=\\!\begin{cases}\frac{\dot{x}^{d}}{\cos(\theta^{d})},&\\!\\!\mbox{if}\sin(\theta^{d})\\!=\\!0\\\
\frac{\dot{y}^{d}}{\sin(\theta^{d})},&\\!\\!\text{otherwise}\end{cases},\,\,\omega^{d}\\!=\\!\begin{cases}\frac{a^{d}}{v^{d}},&\\!\\!\mbox{if}\,v^{d}\neq
0\\\ 0&\\!\\!\text{otherwise}\end{cases},$ (5)

where
$a^{d}=\frac{-\sin({\theta}^{d})\ddot{x}^{d}+\cos(\theta^{d})\ddot{y}^{d}}{v^{d}}$
and $\theta^{d}=\mbox{ATAN2}(\dot{y}^{d},\dot{x}^{d}))$.

###  2.1 Robust Planning

Suppose that a collision-avoiding planner could construct an open loop optimal
state and controls sequences $(\bm{x}^{*}(t),\bm{u}^{*}(t))$ given initial
condition $\bm{x}(0)$, that tracks the desired path $\bm{p}^{d}(t)$ while
avoiding obstacles, for system governed by the nominal dynamics (1).
Notationally, an asterisk $(\cdot)^{*}$ denotes optimality: $\bm{u}^{*}(t)$ is
a sequence of admissible control inputs for finite time or infinite time $t$
that minimizes the error in vehicle position $\bm{p}(t)=[x(t),y(t)]^{T}$
relative to the reference path $\bm{p}^{d}(t)$, for the nominal model (1).
Note, that the planner is expected to deviate from the reference
$\bm{p}^{d}(t)$ to avoid obstacles, such as in [11, 44, 45], based on the
forward propagated ego vehicle trajectory using the nominal model (1).

Being a model-based method, the solution to the finite-time optimal control
problem (FTOCP) may exhibit suboptimal performance or even safety violations
of the actual vehicle in the face of modeling uncertainties and disturbances.

The standard Robust FTOCP formulation involves a minimization-maximization
(min-max) optimization to construct a policy
$\pi:\mathcal{D}^{x}\times\mathbb{Z}\to\mathcal{D}^{u}$ as follows ([46]):

$\displaystyle\pi_{k}\\!=\\!\\!$
$\displaystyle\min_{\\{\bm{u}_{k}\\}^{n_{h}-1}_{k=0}}\max_{\\{\bm{d}_{k}\\}_{k=0}^{n_{h}-1}}\\!\sum_{k=0}^{n_{h}-1}\\!\mathcal{L}_{k}(\bm{x}_{k+1},\bm{u}_{k},\bm{p}^{d}_{k+1})$
(6) s.t.
$\displaystyle\eqref{eq:true},\quad\bm{x}_{k}\in\mathcal{X},\,\bm{u}_{k}\in\mathcal{U},\,\bm{d}_{k}\in\Omega^{d},$
(7)

where $\bm{d}_{k}$ is a discrete-time realization of the model discrepancy
$\tilde{f}$ given nominal state $\bm{x}_{k}$ and input $\bm{u}_{k}$.
Parameters $n_{h}\in\mathbb{Z}_{>0}$ and $\Delta t\in\mathbb{R}_{>0}$ are the
finite-time planning horizon length and time discretization step size. The
stage cost $\mathcal{L}_{k}$ can be a general function such as the squared
distance between the vehicle position and the desired reference. The solution
to (6)-(7) can be computationally intractable [24]. Nevertheless, the
resulting robust FTOCP policy may lead to overly conservative control
sequences.

An alternative solution, which still provides robustness to uncertainty, is to
pair an ancillary controller $\kappa$ for disturbance rejection, with the
open-loop MPC control input:

$\pi=\bm{u}^{*}+\kappa(\bm{x},\bm{x}^{*}),$ (8)

where $\bm{u}^{*}(t)$ and $\bm{x}^{*}(t)$ are the open-loop input and state
trajectories, respectively. The ancillary controller ensures the realized
states $\hat{\bm{x}}$ of the uncertain system remain in a robust control
invariant (RCI) tube around the nominal trajectory $(\bm{x}^{*},\bm{u}^{*})$
from the initial state $\bm{x}_{0}$.

######  Definition 2.

[24] Let $\mathcal{D}^{x}$ be the set of allowable states and define the state
tracking error as $\tilde{\bm{x}}\triangleq\bm{x}-\bm{x}^{*}$. The set
$\Omega_{RCI}\subset\mathcal{X}$ is a robust control invariant (RCI) tube if
there exist an ancillary controller $\kappa(\bm{x},\bm{x}^{*})$ such that if
$\tilde{\bm{x}}(t_{0})\in\Omega_{RCI}$, for all realizations of the
disturbance and model uncertainties, $\tilde{\bm{x}}\in\Omega_{RCI}$, $\forall
t\geq t_{0}$.

To complete the Robust MPC, a constraint-tightened version of the nominal MPC
problem (using the nominal and unperturbed model) can be solved to generate a
robust open-loop trajectory pair $(\bm{x}^{*}_{RCI},\bm{u}^{*}_{RCI})$.

###  2.2 Ancillary Controller - Nominal Model

Since the open-loop inputs $\bm{u}_{d}(t)$ or $\bm{u}^{*}(t)$ can exhibit poor
tracking when applied to the actual vehicle, due to model simplifications and
mismatches, we incorporate tracking error feedback using [43] as the nominal
ancillary controller,

$\kappa(\bm{e})=\delta\bm{u}\triangleq\begin{cases}v=k_{1}\rho\cos{\gamma}\\\
\omega=k_{2}\gamma+k_{1}\frac{\sin(\gamma)\cos(\gamma)}{\gamma}(\gamma+k_{3}\delta)\end{cases},$
(9)

where $k_{1}$, $k_{2}$, and $k_{3}$ are positive constants. This ancillary
controller drives the polar coordinates tracking error to zero with respect to
the nominal model (1). The tracking error in polar coordinates

$\bm{e}=[\rho,\gamma,\delta]^{T}\triangleq f_{\bm{e}}(\bm{x},\bm{x}^{*})$

is a function of the current vehicle state $\bm{x}$ and the optimal state
$\bm{x}^{*}$, see Fig. 2. Polar coordinate tracking error
$\rho\triangleq\sqrt{(x-x^{*})^{2}+(y-y^{*})^{2}}$ is the distance from the
vehicle’s current $(x,y)$ position to a desired waypoint $(x^{*},y^{*})$.
Polar tracking error $\gamma\triangleq\tan^{-1}\frac{y-y^{*}}{x-x^{*}}-\theta$
denotes the angle between the vehicle’s body-fixed $x$-axis (the longitudinal
axis) and a vector that points from the vehicle’s body-fixed frame origin to
the desired waypoint $(x^{*},y^{*})$. Similarly,
$\delta=\gamma+\theta-\theta^{*}$ is the analogous angle between the vehicle’s
body-fixed $x$-axis to the optimal $x$-axis given by $\theta^{*}$. For the
nominal model, the time derivative of the polar coordinates tracking error
$\bm{e}\triangleq[\rho,\gamma,\delta]^{T}$ satisfies:

$\dot{\bm{e}}=\begin{bmatrix}-\cos(\gamma)&\frac{\sin(\gamma)}{\rho}&\frac{\sin(\gamma)}{\rho}\\\
0&-1&0\end{bmatrix}^{T}\delta\bm{u}\triangleq g_{p}(\bm{e})\delta\bm{u}.$ (10)

The vehicle operates in the domain
$\mathcal{D}^{\bm{e}}=\mathcal{D}_{\rho}\times\mathcal{D}_{\gamma}\times\mathcal{D}_{\delta}\subset\mathbb{R}^{3}$
where $\mathcal{D}_{\rho}=(dz,\rho_{max})$,
$\mathcal{D}_{\gamma}=(-\pi/2,\pi/2)$, and
$\mathcal{D}_{\delta}=(-\pi/2,\pi/2)$. 111The vanishingly small interval
$(0,dz)$ is a “deadzone” where the polar coordinate representation (10) is
ill-conditioned as $dz\to 0$. Within domain $\mathcal{D}^{\bm{e}}$, the
function $g_{p}(\bm{e})$ is bounded and Lipschitz continuous with Lipschitz
constant $l_{g_{p}}$. We chose (9) as the ancillary controller because of the
following result, given the operating domain $\mathcal{D}^{\bm{e}}$.

######  Lemma 1.

[47, 43] Consider system (10), control law (9), and positive constants
$k_{1}$, $k_{2}$ and $k_{3}$. The nominal closed-loop system is globally
asymptotically stabilizing to $\bm{e}^{*}=0$.

Lemma 1 can be proved by the following valid and positive definite control
Lyapunov function (CLF):

$V(\bm{e})=\frac{1}{2}(\rho^{2}+\gamma^{2}+k_{3}\delta^{2}).$ (11)

We here showcase several properties of $V(\bm{e})$ which support the main
theorem in Section 5. For quadratic form (11), there exist
$\alpha_{1},\alpha_{2}\in\mathbb{R}_{>0}$ where $\alpha_{1}\|\bm{e}\|_{2}\leq
V\leq\alpha_{2}\|\bm{e}\|_{2}$, $\forall\bm{e}\in{\mathcal{D}}^{\bm{e}}$. Its
time derivative
$\dot{V}=-k_{1}\cos^{2}(\gamma)\rho^{2}-k_{2}\gamma^{2}=-\alpha_{3}(\|\bm{e}\|)$
is negative semi-definite. Adhering to the Lyapunov framework, there exists a
class $\mathcal{K}_{\infty}$ [48] function $\alpha_{3}$ such that
$\dot{V}\leq-\alpha_{3}(V)$. Parameters
$\alpha_{1},\alpha_{2}\in\mathbb{R}_{>0}$, and function $\alpha_{3}(\cdot)$
play an important role in system safety and stability when model (10) is
perturbed by unknown discrepancies, see Section 5.

The nominal trajectory tracking control policy

$\bm{u}=\pi(\bm{e})=\bm{u}^{*}+\kappa(\bm{e})=\bm{u}^{*}+\delta\bm{u}$ (12)

asymptotically tracks $\bm{x}^{*}$ when model mismatches are not present. Our
goal is to construct trajectories and control policies that are robust to
model discrepancies and that provide probabilistic guarantees to avoid a
priori unknown obstacles.

###  2.3 Modeling Discrepancies

We use a probabilistic framework to describe model discrepancies and to define
a probabilistic data-driven upper bound on the sum of all model discrepancies.

######  Definition 3.

Let function $\tilde{f}:Q\to\mathbb{R}^{3}$ measure the difference between the
nominal model (1) and the true model (2). Let $X\in Q$ denote a vehicle
operating state where $Q$ is the vehicle’s operating state space and
$\tilde{f}(X)\in\mathbb{R}^{3}$ denote the additive disturbance at operating
state $X$. We assume that the additive disturbance random variable is drawn
from a probability space $(\Omega,\mathcal{F},\mathbb{P})$, consisting of a
sample space $\Omega$, a $\sigma$-algebra $\mathcal{F}$ over $\Omega$ defining
events, and a probability measure $\mathbb{P}$. We define the minimal model
discrepancy upper bound, $z_{\epsilon}^{\tilde{f}}$, given a risk level
$\epsilon\in[0,1]$ over $\Omega$ for function $\tilde{f}$ as:

$z_{\epsilon}^{\tilde{f}}\triangleq
arg\min_{z\in\mathbb{R}}\\{\mathbb{P}[\|\tilde{f}(X)\|\leq z]\geq
1-\epsilon\\},\quad\forall X\in Q.$ (13)

The value $z_{\epsilon}^{\tilde{f}}$ can be interpreted as the $(1-\epsilon)$
percentile of all possible model discrepancies norms, $\|\tilde{f}(X)\|$, for
all $X\in Q$. Such differences, i.e. residual dynamics, arise from too
simplistic vehicle modeling. The nominal model (1) disregards some robot
configurations (such as different flipper angles) and terrain types (rubber
and foam in our experiments) as illustrated in Fig. 2.

The minimum value in (13) may only be reached in the limit of infinite samples
of $\tilde{f}(X)$, or knowing the probability measure of $X$ on $\Omega$,
which cannot be realized in practice. Instead, we seek to identify a model
discrepancy bound $Z_{\epsilon}^{\tilde{f}}\geq z_{\epsilon}^{\tilde{f}}$
given finite samples of $\bm{x}_{X}$, defined in (3), while making no
assumptions on the distribution over $\Omega$ or the explicit form or
uniqueness of the projection $P_{X\to\bm{x}}$. That is,
$Z_{\epsilon}^{\tilde{f}}$ is a model discrepancy bound, not necessarily the
tightest bound over $\|\tilde{f}(X)\|$ but a realizable bound from a limited
data set.

###  2.4 Obstacle Avoidance

We study the safety-critical autonomy of ground vehicles traversing through
incompletely known terrains populated with stationary but a priori unknown
obstacles. We assume that a 2-D occupancy grid map ($\mathcal{O}_{t}$, derived
from sensor data) is available to the vehicle. The map, defined in inertial
coordinates, has a width and length of $w_{map}$, $l_{map}\in\mathbb{Z}_{>0}$,
respectively, and a grid resolution of $r_{map}$. Each grid cell is associated
with an integer value between $0$ and $100$ that describes the cell occupancy
probabilities where $0$ indicates a $0\%$ chance of obstacle occupancy and
vice versa. Unobserved cells are assigned a risk-neutral $50\%$ occupancy
probability. The map is built incrementally as the robot moves (see Section
7). This work focuses on the dynamic uncertainties arising from model
residuals and ignores possible sensing uncertainties. Our method can be
extended to include measurement uncertainties.

Given this sensor-based grid map of the unknown environment, our framework
allows a ground vehicle with terrain modeling discrepancies to track a
reference trajectory within the map while having the following probabilistic
safety guarantees on collision avoidance.

$\mathbb{P}[\bm{p}_{t}\notin\mathcal{O}]>1-\epsilon\quad\forall t,$ (14)

for any $\bm{p}_{t}\in\mathcal{V}_{t}$ where $\mathcal{V}_{t}$ denotes a
uniform probability distribution over bounded set of $x,y$ positions occupied
the vehicle geometry at time $t$. The probability statement (14) enforces
collision-free given occupied space in $\mathcal{O}$ with confidence
$1-\epsilon$ for all time.

######  Remark 1.

Our analysis focuses on flat terrains populated with stationary obstacles.
Nevertheless, our framework can be readily extended to uneven surfaces using
model (1) or a higher-fidelity model for rough terrain applications, such as
[49], as the nominal model.

##  3 Preliminaries

To ensure vehicle safety in the presence of model uncertainties, we aim to
infer the statistical bounds Eqn. (13) from training data and incorporate this
knowledge into the vehicle’s control and planning strategies. Because
differential-driven ground robots are underactuated, disturbance $\tilde{f}$
cannot be fully compensated via controller design. We formally distinguish
between the components of $\tilde{f}$ that can be “matched” by control inputs
and those that remain “unmatched” due to underactuation. Using closed-loop
tracking data, we identify bounds for the matched and unmatched model
discrepancies using conformal prediction, a statistical inference tool that is
reviewed next. For more details, see [50, 51].

Using the data-driven model discrepancy bound, we formulate an FTOCP that
leverages the quantified uncertainties to synthesize trajectories with
probabilistic safety guarantees. We employ a sampling-based strategy, model
predictive path integral (MPPI), to solve the FTOCP. A full analysis of MPPI
beyond our cursory review can be found in [11, 52].

###  3.1 Matched and Unmatched Discrepancies

We decompose the discrepancy $\tilde{f}$ into a matched component that lies in
the controllable subspace and an unmatched component that lies in the
unmatched subspace, similar to [12, 13, 14].

######  Definition 4.

The additive model uncertainty $\tilde{f}$ can be decomposed into matched
($\tilde{f}_{m}$) and unmatched ($\tilde{f}_{um}$) discrepancies with respect
to reduced-order model (1). The matched discrepancy
$\tilde{f}_{m}:Q\to\mathbb{R}^{3}$ satisfies

$\tilde{f}_{m}(X)\in\mbox{mtch}({g}(\bm{x}_{X})),\quad\forall\,X\in Q,$ (15)

where $\mbox{mtch}(g)\triangleq\mbox{span}(g_{1},g_{2})(\bm{x})$ is the range
space of the actuation matrix of system (1) such that $g_{1}$, $g_{2}$ are the
columns of the actuation matrix $g(\bm{x})$. The unmatched discrepancy is
defined as the function $\tilde{f}_{um}:Q\to\mathbb{R}^{3}$ satisfying

$\tilde{f}_{um}(X)\in\mbox{umtch}({g}(\bm{x}_{X})),\quad\forall\,X\in Q.$ (16)

where
$\mbox{umtch}(g)\triangleq\\{\bm{x}\in\mathcal{D}^{\bm{x}}\,|\,rank(g(\bm{x}))<2\\}$
is the null space of the actuation matrix. For all samples $X\in Q$ and their
respective reduced-order state $\bm{x}_{X}\in\mathcal{D}^{\bm{x}}$, the
summation $\tilde{f}(X)=\tilde{f}_{m}(X)+\tilde{f}_{um}(X)$ holds.

###  3.2 Conformal Prediction

Conformal prediction (CP) provides model- and assumption-free uncertainty
quantification to black-box models[50, 53]. Conformal prediction produces sets
that are guaranteed to contain the desired ground truth on any pre-trained
model while satisfying a user-defined probability. Our pre-trained model is
the nominal model (1). We use CP to calibrate a model discrepancy
$Z_{\epsilon}^{\tilde{f}}\geq z_{\epsilon}^{\tilde{f}}$ with a user-defined
probability $\epsilon\in[0,1]$ without needing to make assumptions on the
probability distribution over the discrepancy sample space.

Let $Y_{0},\ldots,Y_{n-1}$ be $n$ exchangeable random variables where each
$Y_{i}\in\mathbb{R}$ is a nonconformity score. The nonconformity score is
typically chosen to express the difference between the pre-training model of
an unknown system and calibration data (discrepancy ground truth) from that
unknown system. For calibrating model discrepancies $\tilde{f}$ in (2), the
nonconformity score can be a metric measuring its magnitude such as
$Y^{i}\triangleq\|\hat{f}(X_{i})-f(X_{i})\|=\|\tilde{f}(X_{i})\|$. The
conformal prediction algorithm outputs a value
$\overline{Y}_{\epsilon}:\mathbb{R}^{n}\times[0,1]\to\mathbb{R}$ such that the
inequality $\mathbb{P}[Y\leq\overline{Y}_{\epsilon}]\geq 1-\epsilon$ holds
over the sample space of $Y$. Intuitively speaking, a large $Y_{i}$ can be
interpreted as a large model discrepancy, indicating that there is a poor
matching between the nominal and true model.

Conformal prediction is an uncertainty quantification framework built on top
of empirical statistics. Let the cumulative distribution function
$F_{X}\triangleq\mathbb{P}(Y(X)\leq\overline{Y}_{\epsilon})=p$. Let
$Q:[0,1]\to\mathbb{R}$ be a Quantile function, the $(1-\epsilon)^{th}$
quantile returns the value $\overline{Y}_{\epsilon}$ such that
$Y\leq\overline{Y}_{\epsilon}$ for all $X\in\Omega$ as
$Q(1-\epsilon)=F_{X}^{-1}(1-\epsilon)$.

Tibshirani et. al.[54, Lemma 1] relates that the desired
$\overline{Y}_{\epsilon}$ is equivalently the $(1-\epsilon)^{th}$ quantile of
the empirical distribution formed by nonconformity scores
$\\{Y_{0},\ldots,Y_{n-1}$, $\infty\\}$ with an additive $\infty$. To calculate
the $(1-\epsilon)^{th}$ quantile, let $\\{Y_{(0)},\ldots,Y_{(n)}\\}$ be a non-
decreasing sorting of $\\{Y_{0},\ldots,Y_{n-1},\infty\\}$ where $Y_{(i)}\leq
Y_{(i+1)},\forall i\in\mathbb{Z}_{0}^{n}$, also known as the the order
statistics of $\\{Y_{i}\\}_{i=0}^{n}$. The integer
$q_{\epsilon}\triangleq\lceil(n+1)(1-\epsilon)\rceil$ is the order index that
corresponds to the $(1-\epsilon)$ percent confidence level empirically on the
calibration data set, where the multiplier $(n+1)$ is the finite sample
adjustment. The $(1-\epsilon)^{th}$ quantile,
$\overline{Y}_{\epsilon}\triangleq Y_{(q_{\epsilon})}$ defines the desired
$1-\epsilon$ prediction region satisfying the following probability statement:

$\mathbb{P}[\|\tilde{f}(X)\|\geq\overline{Y}_{\epsilon}]\leq\epsilon$ (17)

for any random variable $X\in Q$ and
$\overline{Y}_{\epsilon}=Z_{\epsilon}^{\tilde{f}}$ as desired. The set
$C_{\tilde{f}}(X)\triangleq\\{\bm{y}\in\mathbb{R}^{2}\,|\,\|\bm{y}\|\leq\overline{Y}_{\epsilon}\\}$
is the $(1-\epsilon)$ confidence conformal prediction set.

For the tightness of the conformal probabilistic upper bound, Sadinle et. al.
[55] shows the conformal prediction set $C_{\tilde{f}}$, given
$\epsilon\in[0,1]$, achieves the smallest average set of all possible
prediction schemes $\mathcal{C}$ that offer the desired coverage guarantee if
the sample nonconformity scores $\\{Y_{0},\cdots,Y_{N-1}\\}$ reflects the true
conditional probability:

$\min_{C\in\mathcal{C}}\mathbb{E}[C(X)],\quad\mbox{subject
to}\,\eqref{eq:cp_probability_statement}.$ (18)

###  3.3 Model Predictive Path Integral (MPPI)

MPPI is a sampling-based method, leveraging a duality condition from
information theory, to obtain local optimal controllers given potentially
nonlinear cost and constraints. Consider input sequence
$\\{\bm{u}_{i}\\}_{i=0}^{t}$ where $\bm{u}\in\mathbb{R}^{n_{\bm{u}}}$, we
define $\bm{v}_{i}\triangleq\bm{u}_{i}+\bm{\delta}_{i}$, $\forall
i\in\\{0,\ldots,t\\}$ as the sequence of perturbed inputs where
$\bm{\delta}_{i}$ are zero mean Gaussian input perturbations
$\\{\bm{\delta}_{i}\\}_{i=0}^{t-1}\in\mathcal{N}(0,\Sigma_{\bm{u}})$. We
denote the input trajectories to the algorithm as the following sequences

$\displaystyle V_{t-1}$ $\displaystyle\triangleq$
$\displaystyle\left[\bm{v}_{0},\bm{v}_{0},\cdots,\bm{v}_{t-1}\right]\in\mathbb{R}^{n_{\bm{u}}\times
t},$ (19) $\displaystyle\Delta_{t-1}$ $\displaystyle\triangleq$
$\displaystyle\left[\bm{\delta}_{0},\bm{\delta}_{1},\cdots,\bm{\delta}_{t-1}\right]\in\mathbb{R}^{n_{\bm{u}}\times
t},$ (20) $\displaystyle U_{t-1}$ $\displaystyle\triangleq$
$\displaystyle\left[\bm{u}_{0},\bm{u}_{1},\cdots,\bm{u}_{t-1}\right]\in\mathbb{R}^{n_{\bm{u}}\times
t}.$ (21)

Following the result from [52], the probability density function for $V_{t}$,
denoted as $d_{V|U,\Delta}$, is

$d_{V|U,\Delta}=\left({(2\pi)^{n_{\bm{u}}}|\Sigma_{\bm{u}}|}\right)^{\frac{-t}{2}}\mbox{exp}\left(-\frac{1}{2}\sum_{i=1}^{t}\bm{\delta}_{i}^{T}\Sigma_{\bm{u}}^{-1}\bm{\delta}_{i}\right),$
(22)

with corresponding sample space, denoted as ${\Omega}_{U,\Sigma}$. Similarly,
we denote the probability distribution corresponding to $U_{t}=0$ as
$\Omega_{0,\Sigma}$, known as the base distribution. Consider the stochastic
trajectory optimization problem, adopted from [52]:

$U_{t-1}^{*}=\min_{\begin{smallmatrix}\bm{u}_{i}\in\mathcal{U}\\\ \forall
i\in\mathbb{Z}_{1}^{t}\end{smallmatrix}}\mathbb{E}_{\Omega(U,\Sigma)}\Big{[}\mathcal{L}_{T}(\bm{x}_{t})+\sum_{i=0}^{t-1}\mathcal{L}(\bm{x}_{i},\bm{u}_{i})\Big{]},$
(23)

where $\mathcal{L}_{T}$ and $\mathcal{L}$ respectively denote terminal and
stage costs. Let $\psi_{\bm{x}}(t,{V}_{t-1},\bm{x}_{0})$ be the flow of system
(1) generated by applying input sequence $V_{t}$ from initial condition
$\bm{x}_{0}$.

From an information theoretic perspective, optimization (23) can be converted
into a probability-matching problem [52]. Define the Free Energy
$\mathcal{F_{E}}$ as

$\displaystyle\mathcal{F_{E}}$
$\displaystyle=\log\left(\mathbb{E}_{\Omega_{0,\Sigma}}\Bigg{[}\mbox{exp}\left(-\frac{C_{\bm{x}}(V_{t-1},\bm{x}_{0})}{\lambda}\right)\Bigg{]}\right),$

where $\lambda>0$ is commonly referred to as the inverse temperature, and the
function $C_{\bm{x}}(V_{t-1},\bm{x}_{0})=C_{\bm{x}}$ outputs the cost of the
trajectory generated using input sequence $V_{t-1}$ from initial state
$\bm{x}_{0}$. Using the KL-Divergence properties between two probability
distributions that are absolutely continuous with each other, Williams et.
al.[52] showed that a lower bound to the optimal control problem (23) can be
obtained using the free energy:

$\displaystyle-\lambda\mathcal{F}_{\mathcal{E}}\leq\mathbb{E}_{\Omega_{U,\Sigma}}\left[\mathcal{L}_{T}(\bm{x}_{t})+\sum_{i=0}^{t-1}\mathcal{L}(\bm{x}_{i},\bm{u}_{i})\right].$
(24)

Summarizing the result in [11, 56], the following iterative update law,
building upon importance sampling,

$\displaystyle U_{t}^{*}$
$\displaystyle=U_{t}+\sum_{j=1}^{N_{sample}}w(\Delta^{j}_{t})\Delta_{t}^{j},$
(25) $\displaystyle w(\Delta_{t}^{j})$
$\displaystyle=\frac{1}{\eta}\mbox{exp}\left({-\frac{C_{\bm{x}}}{\lambda}-\sum_{i=0}^{t-1}\bm{u}_{i}^{T}\Sigma_{\bm{u}}^{-1}(\bm{u}_{i}+2\bm{\delta}_{i}^{j})}\right),$
(26)

produces a locally optimal solution to the problem (23). The weighting term
$w\in\mathbb{R}$ characterizes the “importance” of each sampled input
perturbation $\\{\Delta_{t}^{j}\\}_{j=1}^{N_{sample}}$, where $N_{sample}$ is
the number of sampled input perturbations. The parameter $\eta$ can be
approximated as

$\eta\approx\sum_{i=1}^{N_{sample}}\mbox{exp}\left({-\frac{C_{\bm{x}}}{\lambda}-\sum_{i=0}^{t-1}\bm{u}_{i}^{T}\Sigma_{\bm{u}}^{-1}(\bm{u}_{i}+2\bm{\delta}_{i}^{j})}\right)$

using Monte-Carlo estimation [11].

In general, MPPI is a sampling-based alternative to optimization-based MPC
algorithms, flexible to general cost functions and system dynamics. The
aggregation law (25)-(26) is highly parallelizable for online receding-horizon
planning, as demonstrated in UGV racing applications [11].

##  4 Data-Driven Discrepancy Identification

This section summarizes the data-driven identification of a model discrepancy
bound $Z_{\epsilon}^{\tilde{f}}$, where $\tilde{f}$ captures model mismatches
in (2) from sources like slipping, skidding, change in surface traction
properties, input delays, etc. Discrete-time data is captured from the
continuous system at sampling interval $\Delta T$. Time is indicated by an
integer: variable $\bm{x}_{i}$ denotes the value of state $\bm{x}$ at time
$t_{i}=i\Delta T$. $N_{T}$ tuples of training data are gathered from running
the system with the nominal tracking control law (12) and each tuple has the
form

$S_{i}=\\{\bm{x}_{i-1},\hat{\bm{x}}_{i},\bm{x}^{*}_{i},\bm{u}_{i-1},\bm{u}^{*}_{i-1}\\},$
(27)

where $\bm{x}_{i-1}$ is the state at time $t_{i-1}$, and the pair
$(\bm{x}^{*}_{i},\bm{u}^{*}_{i-1})$ is the optimal state and the optimal input
at $t_{i}$ with respect to the cost in (23). This data set is collected from
multiple vehicle trajectories that result from an optimal trajectory planner
(e.g., by a MPC-based planner for dynamics (1)) of the form
$(\bm{x}^{*}_{i},\bm{u}^{*}_{i-1})$ given current state $\bm{x}_{i-1}$.

The actual control input $\bm{u}_{t-1}$ applied to the vehicle in the interval
$[t_{i-1},t_{i}]$ is the sum of the optimal input $\bm{u}^{*}_{t-1}$ and and a
correction term, $\delta\bm{u}_{t-1}$, applied by the ancillary controller
$\eqref{eq:fb_law}$, given the polar coordinate tracking error:
$\bm{u}_{i-1}=\bm{u}^{*}_{i-1}+\delta\bm{u}_{i-1}=\bm{u}^{*}_{i-1}+\kappa(\bm{e}_{i-1})$.
$\hat{x}_{i}$ is the system’s actual state arising from the control input
$\bm{u}_{i-1}$ when it is held constant during $t\in[t_{i-1},t_{i}]$. The
propagation of state $\bm{x}_{i-1}$ with input $\bm{u}_{i-1}$ through nominal
model (1), might be different from the measured $\hat{\bm{x}}_{i-1}$ because
of unmodeled discrepancy $\tilde{f}$. Assuming that the optimal planner and
ancillary controller are recomputed at each time step, we have for the
$j^{th}$ tuple $S_{j}$, ${\bm{x}}_{j-1}=\hat{\bm{x}}_{j}$ holds. Figure 3
overviews the proposed identification process from training data.

Figure 3: An outline of the proposed data-driven discrepancy identification
method. From training data $\mathcal{S}_{i}$ (left panel), we first separate
the closed-loop tracking errors into matched and unmatched model discrepancies
(center panel). Given a user-specified risk tolerance $\epsilon$, conformal
prediction is applied to obtain their probabilistic upper bounds (right).

###  4.1 Discrepancy Identification

To identify the matched and unmatched input discrepancies, we first compute
the nominal model tracking error $\bm{e}_{i-1}$ using $\bm{x}_{i-1}$ and
$\bm{x}^{*}_{i}$ at time $t_{i-1}$. Then, the feedback (measured) tracking
error at time $t_{i}$, denoted as $\hat{\bm{e}}_{i}$, is a function of
$\hat{\bm{x}}_{i}$ and $\bm{x}^{*}_{i}$.

Using (10), we calculate the polar coordinate tracking error $\bm{e}_{L}$ from
the nominal error dynamics (10) at $t_{i}$ given initial condition
$\bm{e}_{i-1}$ and assuming that the ancillary controller $\delta\bm{u}_{i-1}$
satisfies

$\bm{e}_{i}=\int_{t_{i-1}}^{t_{i}}g_{p}({\bm{e}}(\tau))\delta\bm{u}_{i-1}d\tau+{\bm{e}}_{i-1},$
(28)

where $\delta\bm{u}_{i-1}$ is zero-order hold during the interval
$[t_{i-1},t_{i}]$.

Let $\delta{\hat{\bm{e}}}_{i\Delta T}\triangleq\hat{\bm{e}}_{i}-\bm{e}_{i}$ be
the measured tracking error between the true system (2) against the nominal
system (1) given the same initial condition, $\bm{e}_{i-1}$, and the same
input over the interval, $\delta\hat{\bm{e}}_{i\Delta T}$ which can be
decomposed as follows

$\displaystyle\delta{\hat{\bm{e}}}_{i\Delta T}\\!$
$\displaystyle=\\!\\!\int_{t_{i-1}}^{t_{i}}\\!\\!\\!\left(\hat{f}(\hat{\bm{e}}(\tau))\\!-\\!g_{p}({\bm{e}}(\tau))\delta\bm{u}_{i-1}\right)d\tau\\!+\\!\\!\
\underbrace{\delta\bm{e}_{(i-1)\Delta T}}_{=0}$
$\displaystyle=\int_{t_{i-1}}^{t_{i}}g_{p}(\bm{e}(\tau))\bm{d}_{\bm{u}}d\tau+\int_{t_{i-1}}^{t_{i}}\bm{d}_{\bm{u}^{\perp}}d\tau,$
(29)

where $\bm{d}_{\bm{u}}\in\mathbb{R}^{2}$ represents the matched input
discrepancy. The error
$\bm{d}_{\bm{u}^{\perp}}=d^{\perp}\hat{\bm{n}}^{\perp}_{g_{p}}\in\mathbb{R}^{3}$
lies in the null space of $g_{p}$, which is spanned by the unit vector
$\hat{\bm{n}}^{\perp}_{g_{p}}$. Parameter $d^{\perp}\in\mathbb{R}$ is the
magnitude of the unmatched disturbance. By numerical differentiation with
$\Delta T\to 0$, we can identify the perturbed polar coordinate error dynamics
in continuous time:

$\dot{\hat{\bm{e}}}=\dot{\bm{e}}+\delta\dot{\bm{e}}=g_{p}(\hat{\bm{e}})(\delta\bm{u}+\bm{d}_{\bm{u}})+d_{\perp}\hat{\bm{n}}^{\perp}_{g_{p}}.$
(30)

with initial condition $\bm{e}(0)=\hat{\bm{e}}(0)$. For a physical robotic
system, we assume that the dynamics discrepancy is bounded.

######  Assumption 2.

The input (matched) disturbance
$\bm{d}_{\bm{u}}:\hat{\mathcal{D}}^{\bm{e}}\to\mathbb{R}^{2}$ and the
unmatched drift ${d}_{\perp}:\hat{\mathcal{D}}^{\bm{e}}\to\mathbb{R}$ are
bounded.

For safety, the system must be robust up to a user-defined risk tolerance
against the worst-case discrepancies. Therefore, we aim to identify matched
and unmatched discrepancy bounds for both $\|\bm{d}_{\bm{u}}\|$ and
$|{d}_{\perp}|$ using conformal prediction from the finite training data.
Despite [55] addresses that conformal prediction can produce the desired
minimum model discrepancy upper bounds (13), the assumption that the sampled
nonconformity score satisfying (18) is not necessarily valid. Nevertheless,
conformal prediction set coverage guarantees to hold regardless of the
accuracy of the sampled nonconformity scores computed assuming zero model
discrepancy, as they are designed for uncalibrated models [57].

Specifically, we seek to identify the probabilistic bounds on the matched and
unmatched disturbances, satisfying:

$\displaystyle
Z_{\epsilon}\geq\min_{z\in\mathbb{R}}\\{\mathbb{P}_{\hat{\Omega}}[|\|\bm{d}_{\bm{u}}(E)\|\leq
z]\geq 1-\epsilon\\},$ (31) $\displaystyle
Z_{\epsilon}^{\perp}\geq\min_{z\in\mathbb{R}}\\{\mathbb{P}_{\hat{\Omega}}[|{d}_{{\perp}}(E)|\leq
z]\geq 1-\epsilon\\},$ (32)

where $\epsilon\in(0,1)$ is the user-defined risk for all random variables
$E\in\hat{\Omega}$. Physically speaking, $E$ denotes a random sample of polar
tracking error over the set of all possible polar tracking errors by
${\Omega}$. The following subsection details how to find $\|\bm{d}_{\bm{u}}\|$
and $|{d}_{\perp}|$ from the training data and how to calculate $Z_{\epsilon}$
and $Z_{\epsilon}^{\perp}$ using conformal prediction.

###  4.2 Conformal-Driven Discrepancies

For each training state-input tuple, we calculate a tracking error
$\delta\hat{\bm{e}}_{i\Delta T}$, and their the discrepancy bounds
$\|\bm{d}_{\bm{u}}^{i}\|$ and $\|d_{\perp}^{i}\|$ given samples $i$ by solving
the following equations in the given order:

$\displaystyle\|\bm{d}_{\bm{u}}^{i}\|\\!=\\!\max_{\bm{d}_{u}\in\mathbb{R}^{2}}\\!\Bigg{\|}|\delta{\hat{\bm{e}}}_{i\Delta
T}|\\!-\Big{|}\int_{t_{i-1}}^{t_{i}}g_{p}(\bm{e}(\tau))d\tau\bm{d}_{\bm{u}}\Big{|}\Bigg{\|}_{2},$
(33) $\displaystyle|{d}_{\perp}^{i}|=\Bigg{\|}|\delta{\hat{\bm{e}}}_{i\Delta
T}|-\Big{|}\int_{t_{i-1}}^{t_{i}}g_{p}(\bm{e}(\tau))d\tau\bm{d}_{\bm{u}}^{i}\Big{|}\Bigg{\|}_{2}.$
(34)

Since input disturbance $\bm{d}_{\bm{u}}$ can be actively compensated by
control input $\bm{u}$, we identify the largest possible input discrepancy and
allocate the remaining model mismatches to the unmatched drift.

Assume that the training data is sufficiently rich. We randomly sub-sample $L$
values of $\|\bm{d}_{\bm{u}}^{i}\|$ and $\|\bm{d}_{\bm{u}^{\perp}}^{i}\|$
without replacement where $0\ll L<N_{T}$. Algorithm 1 uses training data set
$S_{N_{T}}$ to identify a $(1-\epsilon)$ upper bound
$(Z_{\epsilon},Z^{\perp}_{\epsilon})$ defined in (31) and (32) using the non-
conformity scores $\\{\|\bm{d}_{\bm{u}}^{i}\|\\}_{i=0}^{L-1}$ and
$\\{\|\bm{d}_{\bm{u}^{\perp}}^{i}\|\\}_{i=0}^{L-1}$.

Data: Risk $\epsilon$, sub-sample $L$, training set
$\\{S_{i}\\}_{i=1}^{N_{T}}$.

Result: $Z_{\epsilon}$, $Z_{\epsilon}^{\perp}$ defined by (31)-(32).

Compute index $q_{\epsilon}=\lceil(L+1)(1-\epsilon)\rceil$.

for  _$j$ from $1$ to $L$_ do

Sample $\tilde{\bm{e}}$ without replacement and use (33) and (34) to compute
$\|\bm{d}_{\bm{u}}^{j}\|$, $|d_{\perp}^{j}|$.

end for

Add $\infty$ to the non-conformity scores
$\\{\|\bm{d}_{\bm{u}}^{i}\|\\}_{j=0}^{L-1}$ and
$\\{\|\bm{d}_{\bm{u}^{\perp}}^{j}\|\\}_{j=0}^{L-1}$.

Sort $\\{\|\bm{d}_{\bm{u}}^{j}\|\\}_{j=1}^{L+1}$ and
$\\{|d_{\perp}^{j}|\\}_{j=1}^{L+1}$ in a non-decreasing orders.

$Z_{\epsilon}$, $Z_{\epsilon}^{\perp}$ are the $q_{\epsilon}^{th}$ smallest
value in the sorted lists.

Algorithm 1 Conformal-Driven Discrepancies

Implicitly, the choice of the non-conformity score assumes the discrepancy
noise is uncolored and zero mean. Such an assumption always leads to more
conservative estimates of the discrepancy upper bound for safety-critical
applications. If the noise is known to be colored, a new nonconformity score
that offsets the expected discrepancies,
$\|\tilde{f}(X)-\mathbb{E}_{\Omega}[\tilde{f}(X)]\|_{2}$, could lead to less
conservative identified upper bounds.

In summary, we introduce a distribution-free method to identify the
probabilistic upper bounds for closed-loop tracking input discrepancies
$Z_{\epsilon}$ and the unmatched drift $Z^{\perp}_{\epsilon}$. The following
section augments the ancillary controller using the data-driven discrepancy
bounds to identify the associated maximum trajectory tracking error.

##  5 Maximum Tracking Error Tubes

In addition to the identified error dynamics
$\eqref{eq:preturbed_tracking_eqn}$, we also introduce the following perturbed
variations of the nominal error dynamics $\eqref{eq:polar_eom}$,

$\displaystyle\dot{\tilde{\bm{e}}}$
$\displaystyle=g_{p}(\tilde{\bm{e}})(\delta\bm{u}+\bm{d}_{\bm{u}}),$ (35)
$\displaystyle\dot{\overline{\bm{e}}}$
$\displaystyle=g_{p}(\overline{\bm{e}})\delta\bm{u}+{d}^{\perp}\hat{\bm{n}}_{g_{p}}^{\perp},$
(36)

where $\bm{e}(0)=\tilde{\bm{e}}(0)=\overline{\bm{e}}(0)=\hat{\bm{e}}(0)$. The
perturbed dynamics (35) corresponds to the case where there are no unmatched
discrepancies, i.e. $d^{\perp}(t)=0,\forall t$. Similarly, the dynamics of
$\overline{\bm{e}}$ corresponds to the case where there is no matched
uncertainty along the controllable subspace, i.e.
$\bm{d}_{\bm{u}}(t)=0,\forall t$. To study the effect of the discrepancies, we
introduce the following lemmas, which leverage ideas from input-to-state
stability.

######  Lemma 2.

Consider the perturbed error dynamics (35). Assume a positive constant
$Z_{\epsilon}$ exists and satisfies Eq. (31) for $t\in[0,\Delta T]$ as
$\epsilon\to 0$. Define the set:

$\Omega_{ISS}=\Big{\\{}\tilde{\bm{e}}\in\hat{\mathcal{D}}^{\tilde{\bm{e}}}\,\Big{|}\,V(\tilde{\bm{e}})\leq
Z_{\epsilon}^{2}/4\Big{\\}},$ (37)

where $V$ is the positive definite function (11). Consider the augmented
ancillary control law,

$\kappa_{ISS}(\tilde{\bm{e}})=\kappa(\tilde{\bm{e}})-\frac{1}{\lambda_{1}}g_{p}(\tilde{\bm{e}})^{T}\tilde{\bm{e}}.$
(38)

For any $\tilde{\bm{e}}(0)\in\Omega_{ISS}$, if there exists a $\lambda_{1}>0$
such that applying the augmented control law (37) yields the inequality
$-\alpha_{3}(\alpha_{2}^{-1}(V(\tilde{\bm{e}})))+\lambda_{1}V(\tilde{\bm{e}})\leq-\tilde{\alpha}_{3}({V}(\tilde{\bm{e}}))$,
where $\tilde{\alpha}_{3}$ is a class $\mathcal{K}_{\infty}$ function, for all
$\tilde{\bm{e}}\in\Omega_{ISS}$, then
$\psi_{\tilde{\bm{e}}}(t,\kappa_{ISS}(\tilde{\bm{e}}),\tilde{\bm{e}}(0))\in\Omega_{ISS}(t)$
222$\psi_{\tilde{\bm{e}}}(t,\kappa_{ISS}(\tilde{\bm{e}}),\tilde{\bm{e}}(0))$
is the solution (flow) of $\tilde{\bm{e}}$ governed by eqn. (35) under the
augmented control law $\kappa_{ISS}$ with the initial value
$\psi(0)=\tilde{\bm{e}}(0)$. for all $t\in(0,\Delta T)$.

###### Proof.

From the definition of the positive definite function $V$ (11) and with the
augmented controller $\kappa_{ISS}$, we have

$\displaystyle\dot{V}$
$\displaystyle\leq-\alpha_{3}(\|\tilde{\bm{e}}\|)-\frac{1}{\lambda_{1}}\tilde{\bm{e}}^{T}g_{p}(\tilde{\bm{e}})g_{p}(\tilde{\bm{e}})^{T}\tilde{\bm{e}}+\tilde{\bm{e}}^{T}g_{p}(\tilde{\bm{e}})\bm{d}_{\bm{u}}$
$\displaystyle\leq-\alpha_{3}(\|\tilde{\bm{e}}\|)-\frac{1}{\lambda_{1}}\|g_{p}(\tilde{\bm{e}})^{T}\tilde{\bm{e}}\|_{2}^{2}+|\tilde{\bm{e}}^{T}g_{p}(\tilde{\bm{e}})\bm{d}_{\bm{u}}|$
$\displaystyle\leq-\alpha_{3}(\|\tilde{\bm{e}}\|)-\frac{1}{\lambda_{1}}\Big{\|}g_{p}^{T}\tilde{\bm{e}}+\frac{\lambda_{1}}{2}\bm{d}_{\bm{u}}\Big{\|}^{2}_{2}+\frac{\lambda_{1}(Z_{\epsilon})^{2}}{4}$
$\displaystyle\leq-\alpha_{3}(\|\tilde{\bm{e}}\|)+\frac{\lambda_{1}(Z_{\epsilon})^{2}}{4}.$

Given $\lambda_{1}>0$ exists, the following inequalities hold

$\displaystyle\dot{V}$
$\displaystyle\leq-\alpha_{3}(\|\tilde{\bm{e}}\|)+\lambda_{1}V(\tilde{\bm{e}})$
$\displaystyle\leq-\alpha_{3}(\alpha_{2}^{-1}(V(\tilde{\bm{e}}))+\lambda_{1}V(\tilde{\bm{e}})\leq-\tilde{\alpha}_{3}(V(\tilde{\bm{e}}))$

given perturbed dynamics (35). With
$V(\bm{e}(0))\leq\frac{Z_{\epsilon}^{2}}{4}$, we have
$V(\tilde{\bm{e}}(t))\leq\frac{Z_{\epsilon}^{2}}{4},\forall t$, i.e.
$\psi_{\tilde{\bm{e}}}(t,\kappa_{ISS}(\tilde{\bm{e})},\tilde{\bm{e}}(0))\in\Omega_{ISS}$.
∎

It is important to recognize the set $\Omega_{ISS}$ is a Robust Control
Invariant (RCI) set. Lemma 2 only considers the case where all disturbances
lie in the controllable subspace. For the complementary case governed by (36),
we leverage the Gronwall-Bellman inequality to deduce the following result.

######  Lemma 3.

Under the nominal dynamical system (10), the perturbed dynamical system (36)
controlled by the nominal feedback control law $\bm{u}=\kappa(\bm{e})$ in Eqn.
(9) has the closed-loop dynamics

$\displaystyle\dot{\bm{e}}$
$\displaystyle=g_{p}(\bm{e})\kappa(\bm{e})=f_{cl}(\bm{e}),$
$\displaystyle\dot{\overline{\bm{e}}}$
$\displaystyle=g_{p}(\overline{\bm{e}})\kappa(\overline{\bm{e}})+{d}^{\perp}\hat{\bm{n}}_{g_{p}}^{\perp}={f}_{cl}(\overline{\bm{e}})+{d}^{\perp}\hat{\bm{n}}_{g_{p}}^{\perp}.$

Let $Z_{\epsilon}^{\perp}\in\mathbb{R}_{>0}$ satisfy (32). Suppose there
exists a Lipschitz constant $l_{cl}$ with respect to the domain
$\hat{\mathcal{D}}^{\bm{e}}$ for function $f_{cl}$. Let
$\psi_{\bm{e}}(t,\kappa(\bm{e}),\bm{e}(0))$ and
$\psi_{\overline{\bm{e}}}(t,\kappa(\overline{\bm{e}})),\overline{\bm{e}}(0))$
be the flows of the systems (10) and (36). The following inequality holds for
all $t\in[0,\Delta T]$:

$\|\psi_{\bm{e}}(t,\kappa(\bm{e}),\bm{e}(0))-\psi_{\overline{\bm{e}}}(t,\kappa(\overline{\bm{e}})),\overline{\bm{e}}(0))\|\leq\\\
Z^{\perp}_{\epsilon}te^{l_{cl}t}+\|\bm{e}(0))-\overline{\bm{e}}(0))\|e^{l_{cl}t}.$
(39)

###### Proof.

By forward integration, the flows satisfy

$\displaystyle\|\psi_{\bm{e}}(t,\kappa(\bm{e}),\bm{e}(0))-\psi_{\overline{\bm{e}}}(t,\kappa(\overline{\bm{e}})),\overline{\bm{e}}(0))\|$
$\displaystyle\leq\|\bm{e}(0))-\overline{\bm{e}}(0))\|\\!+\\!\\!\int_{0}^{t}\\!\\!\big{\|}f_{cl}(\bm{e})\\!-\\!f_{cl}(\overline{\bm{e}})\\!-\\!{d}^{\perp}\hat{\bm{n}}_{g_{p}}^{\perp}\big{\|}d\tau$
$\displaystyle\leq\|\bm{e}(0))-\overline{\bm{e}}(0))\|+\\!\\!\int_{0}^{t}\\!\\!l_{cl}\|\bm{e}(\tau)-\overline{\bm{e}}(\tau)\|d\tau\\!+\\!Z^{\perp}_{\epsilon}t.$

The Gronwall-Bellman Inequality [58] yields (39). ∎

Figure 4: Given reference path $\bm{p}^{d}(t)$ and optimal trajectory
$\\{\bm{x}^{*}_{k},\bm{u}^{*}_{k}\\}_{k=1}^{n_{h}}$, the maximum tracking
error under augmented policy $\pi_{ISS}$ is depicted. By converting the
optimal state $\bm{x}_{1}^{*}$ and current state $\bm{x}_{0}$ into polar
coordinate error $\bm{e}_{0}$, the tracking problem reduces to the
stabilization of the error $\bm{e}_{eq}=0$. Theorem 1 proves that if
$\|\bm{e}_{0}\|_{2}\leq r_{0}$, then the maximum tracking deviation during
$t\in[0,\Delta T]$ is $\sup_{t\in[0,\Delta T]}\|\bm{e}(t)\|_{2}$ is upper
bounded by $r_{\Delta T}$.

Intuitively speaking, the result of Lemma 3 states that if there is unmatched
drift present, then the closed-loop system trajectory between the perturbed
and nominal system grows exponentially in time. Building up on Lemma 1 and
Lemma 3, our main contribution deduces an augmented ancillary controller and
the associated maximum discrepancy tube for the perturbed system (30).

######  Theorem 1.

Suppose the system (10) is perturbed by input disturbance $\bm{d}_{\bm{u}}$
and unmatched drift $d_{\perp}$, as in (30), where the disturbances
$\bm{d}_{\bm{u}}$ and $d_{\perp}$ satisfy Assumption 2 given domain
$\mathcal{D}^{\bm{e}}$. Given $\epsilon\to 0$, let $Z_{\epsilon}$ and
$Z_{\epsilon}^{\perp}$ for $t\in[0,\Delta T]$ being the discrepancy upper
bounds that also satisfy (31) and (32), respectively.

Let $l_{V}$ be the Lipschitz constant for the function ${V}$ in Eqn. (11) on
the bounded set $\hat{\mathcal{D}}^{\bm{e}}$. Let $\Delta T$ be small enough
to satisfy $Z_{\epsilon}^{\perp}\Delta T\exp(l_{V}\Delta T)\leq\alpha_{1}$.
Consider a time varying radius $r(\tau)$ that satisfies

$r(\tau)=r_{\tau}\triangleq\frac{(Z_{\epsilon})^{2}}{4(\alpha_{1}-Z_{\epsilon}^{\perp}\tau
e^{l_{V}\tau})}.$ (40)

Consider the augmented ancillary controller (38) and error dynamics
$\dot{\bm{e}}\triangleq g_{p}(\bm{e})(\kappa_{ISS}(\bm{e})+\bm{d}_{\bm{u}})$
and $\dot{\hat{\bm{e}}}\triangleq
g_{p}(\hat{\bm{e}})(\kappa_{ISS}(\hat{\bm{e}})+\bm{d}_{\bm{u}})+{d}^{\perp}\hat{\bm{n}}_{g_{p}}^{\perp}$,
where $\bm{d}_{\bm{u}}(0)=0$, ${d}_{\perp}(0)=0$ and $\|\bm{e}(0)\|\leq r(0)$.
The existence of a $\hat{\lambda}_{1}>0$ such that
$-{\alpha}_{3}(\alpha_{2}^{-1}(V(\hat{\bm{e}})))+\hat{\lambda}_{1}V(\hat{\bm{e}})\leq-\hat{\alpha}_{3}(V(\hat{\bm{e}}))$
is true for all $t\in[0,\Delta T]$, where $\hat{\alpha}_{3}$ is a class
$\mathcal{K}_{\infty}$ function, implies that the flow of the closed-loop
333Referring to the error dynamics under augmented control law
$\bm{u}=\kappa_{ISS}(\hat{e})$ (38) with $\hat{\lambda}_{1}$ replacing
$\lambda_{1}$ tracking error $\hat{\bm{e}}(t)$ is bounded by
$\|\hat{\bm{e}}(t)\|\leq r(t)$ over $t\in[0,\Delta T]$.

###### Proof.

The continuous and differentiable function $\dot{V}$ is bounded over compact
set $\hat{\mathcal{D}}^{\bm{e}}$ from Assumption 2. Therefore, $V$ is
Lipschitz continuous over domain $\hat{\mathcal{D}}^{\bm{e}}$. If there exist
$\tilde{\lambda}_{1}>0$, then under $\kappa_{ISS}$ yields

$\displaystyle\\!\dot{V}(\hat{e})\\!\leq\\!-\alpha_{3}(\|\hat{\bm{e}}\|)\\!+\\!\frac{\hat{\lambda}_{1}Z_{\epsilon}^{2}}{4}\\!+\\!|\hat{\bm{e}}^{T}\bm{d}_{\bm{u}}^{\perp}|\\!\leq\\!-\hat{\alpha}_{3}(V(\hat{\bm{e}}))\\!+\\!\|\hat{\bm{e}}\|_{2}Z_{\epsilon}^{\perp}\\!.$

The first inequality follows the proof of Lemma 2 with the addition of
$\hat{\bm{e}}^{T}\bm{d}_{u}^{\perp}\leq|\hat{\bm{e}}^{T}\bm{d}_{u}^{\perp}|$.
The second inequality holds from a combination of the existence of
$\hat{\lambda}_{1}$, the definition of $Z_{\epsilon}^{\perp}$ (32), and
Holder’s inequality. From Lemma 3, we have

$\sup_{\tau\in[0,t]}|V(\bm{e}(\tau))-V(\hat{\bm{e}}(\tau))|\leq\left(\sup_{\tau\in[0,t]}\|\hat{\bm{e}}\|_{2}\,Z_{\epsilon}^{\perp}\tau\right)e^{l_{V}\tau},$

where $|V(\bm{e}(0))-V(\hat{\bm{e}}(0))|=0$ because
$\bm{e}(0)=\hat{\bm{e}}(0)$. By adding and subtracting zero and triangle
inequalities, we have inequalities
$V(\hat{\bm{e}}(\tau))\leq|V(\hat{\bm{e}}(\tau))|\leq|V(\bm{e}(\tau))|+|V(\hat{\bm{e}}(\tau))-V(\bm{e}(\tau))|$.
Recall the robust control invariance set (37). Let the initial error radius
$r(0)$ take the explicit value

$r(0)=\frac{(Z_{\epsilon})^{2}}{4\alpha_{1}^{-1}}.$ (41)

Further, since $\|\bm{e}(0)\|\leq r(0)$ and there exists a
$\hat{\lambda}_{1}>0$, the unperturbed error dynamics satisfies
$\|\bm{e}(t))\|\leq r(0)$ for all $t\in[0,\Delta T]$, by Lemma 1. Combining
these inequalities, we can show

$\displaystyle\sup_{\tau\in[0,t]}\\!\\!V(\hat{\bm{e}}(\tau))\\!\leq\\!\alpha_{1}r_{0}\\!+\\!\left(\sup_{t\in[0,\Delta
T]}\\!\|\hat{\bm{e}}(t)\|Z^{\perp}_{\epsilon}\Delta T\right)\\!e^{l_{V}\Delta
T}.$ (42)

because $V(\hat{\bm{e}})\geq\alpha_{1}\|\hat{\bm{e}}\|$. Substituting this
inequality into equation (42), we arrive at

$\\!\\!\\!\sup_{\tau\in[0,t]}\\!\|\hat{\bm{e}}(\tau)\|\\!\leq\\!r_{0}\\!+\\!\\!\sup_{\tau\in[0,t]}\\!\\!\|\hat{\bm{e}}(\tau)\|\frac{Z_{\epsilon}^{\perp}\tau
e^{l_{V}\tau}}{\alpha_{1}}.$ (43)

Lastly, combining equations (43) and (41) leads to the desired maximum
tracking error bound (40). ∎

The increasing tube (radius-wise) defined by time-dependent radius $r(t)$ (40)
in the interval $t\in[0,\Delta T]$ is the maximum tracking error tube,
illustrated in Figure 4. Note, every planned desired trajectory
$\bm{x}^{*}(t)$ resulting from desired inputs $\bm{u}^{*}(t)$ starting at
state $\bm{x}_{0}$ is equivalent to stabilizing the tracking error from
$\bm{e}_{0}$ to $[0,0,0]^{T}$. Leveraging the result of Theorem 1, the
perturbed model tracking error $e(t)$ within each planning interval
$t\in[0,\Delta T]$ will be inside a growing tube along the planned trajectory
$\bm{x}^{*}(t)$ for $t\in[0,\Delta T]$ under the augmented controller
$\kappa_{ISS}$, if the initial tracking error $\bm{e}_{0}$ is inside a ball of
radius $r_{0}$ $\eqref{eq:r0_definition}$, as illustrated in Fig. 4.

######  Remark 2.

The condition $\epsilon\to 0$ can require
$Z_{\epsilon},Z_{\epsilon}^{\perp}\to\infty$ which is unpractical for robotic
applications. Therefore for $\epsilon\in(0,1)$, one can translate the result
of Theorem 1 as the inequality $\eqref{eq:main_result}$ is true for
$(1-\epsilon)$ confidence level.

##  6 Discrepancy-Aware Planning

Because differential-drive ground vehicles are underactuated, the unmatched
uncertainties cannot be compensated in a controller synthesize problem.
Nevertheless, the closed-loop trajectory drift (Lemma 3) can be effectively
modulated through trajectory re-planning such as through heading adjustments,
as demonstrated by the tube-MPC method [59].

To complete our framework, this section proposes a novel Discrepancy-Aware
Planning algorithm that optimizes reference tracking while providing safety
constraint satisfaction at a desired risk tolerance. We focus on two
challenges: integrating maximum tracking error into the occupancy map to infer
probabilistic safe traversal and finding optimal trajectories and inputs for
the policy (8).

Figure 5: (Left) Configuration B test set up capture where three stationary
obstacles are placed such that the vehicle needs to deviate from its reference
trajectory to avoid. (Right) An overlay of the raw occupancy map and the
discrepancy-aware cost map $\mathcal{C}_{\epsilon}$ with
$\epsilon\in(0.0013,0.0016)$ resulting a $N_{\epsilon}=16$ composed by
$r_{ego}=0.40~{}\mbox{m}$ and $r_{buff}=0.15~{}\mbox{m}$ with
$r_{map}=0.05~{}\mbox{m}$.

###  6.1 Probabilistic Safety-Critical Planning

Given a desired trajectory, the optimal tracking input can be found using a
deterministic FTOCP with the cost:

$\\!\mathcal{L}_{track}=\\!\mathcal{L}_{T}(\delta\bm{p}_{t+n_{h}})\\!+\\!\sum_{k=0}^{n^{h}-1}\left(\mathcal{L}_{s}(\delta\bm{p}_{k})+\frac{1}{2}\bm{u}_{k}^{T}R\bm{u}_{k}\right)$
(44)

where $\delta\bm{p}_{k}=\bm{p}_{k}-\bm{p}^{d}_{k}$ is the output (position)
tracking error in a fixed frame. Functions
$\mathcal{L}_{T}\triangleq\delta\bm{p}_{t+{n}^{h}}^{T}Q_{T}\delta\bm{p}_{t+{n}^{h}}$
and $\mathcal{L}_{s}\triangleq\delta\bm{p}_{k}^{T}Q\delta\bm{p}_{k}$ for
$k\in\\{0,\ldots,n_{h}-1\\}$ are convex terminal and stage costs,
respectively. Matrices $Q_{T}$, $Q$, $R\in\mathbb{R}^{2\times 2}$ are positive
definite. Note, we apply a maximum tracking cost threshold,
$\overline{\mathcal{L}}_{track}\in\mathbb{R}_{>0}$ 444The parameter
$\overline{\mathcal{L}}_{track}$ stops the cost-to-go from becoming unbounded
as the output tracking error increases which needs to be tuned. The authors
infer $\overline{\mathcal{L}}_{track}$ from the system’s input limits and the
planning horizon such that the maximum stage cost corresponding to the
physical limits imposed by the robotic hardware., to the cost as
$\mathcal{L}_{track}=\min(\mathcal{L}_{track},\overline{\mathcal{L}}_{track})$.

Given a vehicle operating state $X_{t}$ and its projected nominal state
$\bm{x}_{t}$, the solution to the following discrete-time optimization problem
yields the desired probabilistically safe input sequence

$\displaystyle\\{\bm{u}^{*}_{t+k}\\}$
${}_{k=0}^{n_{h}-1}=\\!\\!\\!\\!\min_{\tiny\begin{array}[]{c}\\{\bm{u}_{t+k}\\}^{n_{h}-1}_{k=0}\end{array}}\normalsize\\!\\!\\!\sum_{k=0}^{n_{h}-1}\mathcal{L}_{track}$
(45b) s.t. $\displaystyle X_{t+k+1}=\hat{f}^{d}_{t+k}(X_{t+k})$ (45c)
$\displaystyle\bm{x}_{t+k+1}=P_{X\to\bm{x}}(X_{t+k+1})\in\mathcal{D}^{\bm{u}}$
(45d)
$\displaystyle\bm{u}_{t+k}=P_{X\to\bm{u}}(X_{t+k})\in\mathcal{D}^{\bm{u}},\quad\forall
k\in\mathbb{Z}_{0}^{n_{h}-1}$ (45e)
$\displaystyle\bm{x}_{t}=P_{X\to\bm{x}}({\bm{x}}_{t})$ (45f)
$\displaystyle\mathbb{P}_{\Omega}[M\bm{x}_{t+1}\in\overline{\mathcal{O}}]\leq\epsilon,$
(45g)

that avoids all occupied grids in the map $\mathcal{O}$ at a minimum
probability $(1-\epsilon)$ percent. Equation (45c) represents a discrete-time
update of the true system operating state $X$ with time discretization step
size $\Delta T$. The matrix $M\in\mathbb{R}^{2\times 3}$ maps from nominal
states $\bm{x}$ to vehicle positions $\bm{p}$, and constraint (45f) means the
initial condition is set by the current state. Inequality (45g) constrains the
probability of the planned vehicle’s position, at time $t+1$ given the current
nominal state $\bm{x}_{t}$, to be inside an augmented occupied set, denoted as
$\overline{\mathcal{O}}\subseteq\mathbb{R}^{2}$, is less than $\epsilon$. The
set $\overline{\mathcal{O}}$ is constructed from $\mathcal{O}$ with the
addition of the vehicle geometry such that if the point mass at position
$\bm{p}\in\mathbb{R}^{2}$ satisfied $\bm{p}\notin\overline{\mathcal{O}}$, then
the vehicle occupied points given center position $\bm{p}$ denoted as the set
$\mathcal{S}_{\bm{p}}\subset\mathbb{R}^{2}$ satisfied
$\mathcal{S}_{\bm{p}}\cap\mathcal{O}=\emptyset$. Physically speaking, the
vehicle should not enter the obstacle-occupied grids given by the map
$\mathcal{O}$ more frequently than the specified risk tolerance.

The optimization problem (45) is difficult to solve because the true system
dynamics (45c) is unknown and unobservable, constraint (45g) is
nondeterministic, and the occupancy percentage given map $\mathcal{O}$ given
continuous position $x,y$ is discontinuous. As a solution, we proposed to
reformulate (45) into a deterministic program while providing the same or
lower obstacle collision tolerance $\epsilon$.

Recall that polar coordinates state $\rho$ represents the displacement between
the vehicle’s actual position and the planned one. From Theorem 1, we have

$\sup_{t\in[0,\Delta]}|\hat{\rho}(t)|\\!=\\!\\!\sup_{t\in[0,\Delta
T]}\\!\\!\\!|\bm{i}^{T}\hat{\bm{e}}(t)|\leq\|\bm{i}\|\\!\\!\sup_{t\in[0,\Delta
T]}\\!\\!\\!\|\hat{\bm{e}}(t)\|\\!\leq\\!r_{\Delta T},$

where $\bm{i}=[1,0,0]^{T}$. Given the current nominal state $\bm{x}_{t}$, if
the initial tracking error arising from the planned state $\bm{x}_{t+1}^{*}$
satisfies $\|\bm{e}_{0}\|\leq r_{0}$, we can then guarantee up to
$(1-\epsilon)$ confidence that vehicle position $\bm{p}_{t+1}$, at time
$t+\Delta t$, is an element of the following set

$\mathcal{S}_{ISS}=\\{\bm{p}\in\mathbb{R}^{2}\,|\,\|\bm{p}_{t+1}(\bm{x}_{t},\kappa(\bm{e}_{0}))-\bm{p}^{*}_{t+1}\|\leq
r_{\Delta T}\\}.$ (46)

That is, if the constraint $\|\bm{e}_{0}(\bm{x}_{t},\bm{x}_{t+1}^{*})\|\leq
r_{0}$ is satisfied, then enforcing an added distance gap of $r_{\Delta t}$
between the ego vehicle and the obstacle is sufficient to guarantee obstacle
avoidance with a $(1-\epsilon)$ confidence.

######  Theorem 2.

Consider a differential-driven UGV with nominal dynamics (1), true dynamics
(2), and reference trajectory $\bm{p}^{d}(t)$. Denote the current vehicle
operating state as $X_{t}$ and its projected nominal state as $\bm{x}_{t}$.
The polar tracking error satisfies nominal dynamics (10). Consider the
tracking control policy (8) where the nominal ancillary controller $\kappa$
(9) is used for stabilizing the tracking error $\bm{e}$.

Suppose (10) is perturbed by an additive matched disturbance,
$g_{p}(\bm{e})\bm{d}_{\bm{u}}$, and an additive unmatched model disturbance
$d_{\perp}\hat{\bm{n}}_{g_{p}}^{\perp}$ to (30), and there exist
$Z_{\epsilon}\in\mathbb{R}_{>0}$ satisfying (31) and
$Z_{\epsilon}^{\perp}\in\mathbb{R}_{>0}$ satisfying (32) given a risk
tolerance $\epsilon\in[0,1]$. Suppose there exists
$\tilde{\lambda}_{1}\in\mathbb{R}_{>0}$ in Theorem 1. Let
$\\{\mathcal{FS}^{*}_{t+k}\\}_{k=0}^{n_{h}-1}$ be the set of admissible inputs
555 If a input sequences $\\{\bm{u}_{t+k}\\}_{k=0}^{n_{h}-1}$ satisfies the
constraints (47c) - (47g) given current state $\bm{x}_{t}$ then such input
sequence is an element of the set
$\\{\mathcal{FS}^{*}_{t+k}\\}_{k=0}^{n_{h}-1}$,
$\bm{u}_{t+k}\in\mathcal{FS}^{*}_{t+k}$ for all
$k\in\mathbb{Z}_{k=0}^{n_{h}-1}$. for the following optimization problem.

$\displaystyle\\{\bm{u}^{*}_{t+k}\\}$
${}_{k=0}^{n_{h}-1}=\\!\\!\\!\\!\min_{\tiny\begin{array}[]{c}\\{\bm{u}_{t+k}\\}^{n_{h}-1}_{k=0}\end{array}}\normalsize\\!\\!\\!\sum_{k=0}^{n_{h}-1}\mathcal{L}_{track}$
(47b) s.t.
$\displaystyle\bm{x}_{t+k+1}=f^{d}_{t+k}(\bm{x}_{t+k},\bm{u}_{t+k},\Delta t)$
(47c) $\displaystyle\bm{e}_{k}=f_{\bm{e}}(\bm{x}_{t+k},\bm{x}_{t+k+1})$ (47d)
$\displaystyle\|\bm{e}_{k}\|\leq r_{0}$ (47e)
$\displaystyle{C}(\bm{p}_{t+k+1},r_{(k+1)\Delta
T})\cap\overline{\mathcal{O}}=\emptyset$ (47f)
$\displaystyle\bm{u}_{t+k}+\kappa_{ISS}(\bm{e}_{k})\in\mathcal{D}^{\bm{u}},\quad\forall
k\in\mathbb{Z}_{0}^{n_{h}-1}$ (47g) $\displaystyle\bm{x}_{t}={\bm{x}}_{t},$
(47h)

where equality constraint (47c) is the discrete-time version of nominal model
(1). The set ${C}(\bm{p}_{k+1},r_{\Delta T})$ in (47f) denotes the closed disc
with center $\bm{p}_{k+1}\in\mathbb{R}^{2}$ and radius $r_{\Delta
T(k+1)}\in\mathbb{R}_{>0}$. Let $\\{\mathcal{FS}_{k}\\}_{k=0}^{n_{h}-1}$ be
the sequence of feasible input sets to the optimization problem (45). If
$r_{t}$ is computed using (40), then
$\mathcal{FS}^{*}_{t+k}\subseteq\mathcal{FS}_{t+k}$, for all
$k\in\mathbb{Z}_{0}^{n_{h}-1}$.

###### Proof.

As an overview, we show set inclusion using a standard approach by showing
every element of the set $\mathcal{FS}^{*}_{k}$ also belongs to the set
$\mathcal{FS}_{k}$ for all $k\in\mathbb{Z}_{0}^{n_{h}-1}$. Consider $k=0$.
From Theorem 1, under the augmented ancillary controller $\kappa_{ISS}$, we
can assert that the position flow of the perturbed system
$\bm{\psi}_{\bm{p}}(\tau)$ for $\tau\in[t,t+\Delta T]$ under any input
$\bm{u}_{t}\in\mathcal{FS}^{*}_{t}+k$ with initial position $\bm{p}_{t}$ and
$(1-\epsilon)$ confidence, satisfies the following,

$\displaystyle\bm{\psi}_{\bm{p}}(\tau)\\!-\\!\bm{p}^{*}(\tau)\leq\\!\\!\\!\\!\sup_{\tau\in[t,t+\Delta
T]}\\!\\!\|\delta\bm{p}(\tau)\|\leq\\!\\!\\!\\!\sup_{\tau\in[t,t+\Delta
T]}\\!\\!|\rho(\tau)|\leq r_{\Delta T}.$

The above inequality implies, for all $\bm{u}_{t}\in\mathcal{FS}^{*}_{0}$,
satisfaction of the inequality constraints (47e) and (45e) is equivalent to
the statement that the perturbed system’s position flow
$\bm{\psi}_{\bm{p}}(\tau)$ for $\tau\in[t,t+\Delta T]$ does not intersect an
occupied grid with $(1-\epsilon)$ confidence. Therefore, control inputs
$\bm{u}_{t}\in\mathcal{FS}_{t}^{*}$ are also feasible solutions to
optimization problem (45) yielding $\bm{u}_{t}\in\mathcal{FS}_{t}$, meaning
$\mathcal{FS}^{*}_{0}\subseteq\mathcal{FS}_{0}$. For $k=1$, we can similarly
inspect the flow generated by the composite input

$\displaystyle\bm{u}(\tau)=\begin{cases}\bm{u}_{t}\in\mathcal{FS}^{*}_{t},\quad&\tau\in[t,t+\Delta
T]\\\ \bm{u}_{t+1}\in\mathcal{FS}^{*}_{t+1},\quad&\tau\in[t+\Delta T,t+2\Delta
T].\end{cases}$

Building on the result for $k=0$, we have
$\bm{\psi}_{\bm{p}}(\tau)\leq\bm{p}^{*}(\tau)+r_{2\Delta T}$ for
$\tau\in[t,t+2\Delta T]$ with confidence $1-\epsilon$. Therefore,
$\forall\bm{u}_{t+1}\in\mathcal{FS}^{*}_{t+1}$ are also member of set
$\mathcal{FS}_{t+1}$, yielding
$\mathcal{FS}^{*}_{t+1}\subseteq\mathcal{FS}_{t+1}$. The proof for the
remaining $k\in\mathbb{Z}_{k=2}^{n_{h}-1}$ follows by induction. ∎

Based on Theorem 2, we now can solve the deterministic optimization problem
(47) with an inaccurate but known nominal model so that it guarantees the
desired obstacle avoidance behavior and satisfies the original optimization
problem (45). Lastly, we assume that the optimization-based planner receives
measurement updates at every $\Delta T$ seconds. Following standard receding
horizon MPC implementation, the planner replans at the same rates as
measurement updates with constraint (47e) only applied at $k=0$ 666Enforcing
trajectory level safety, by adding safety distance of $r_{(k+1)\Delta T}$ at
future horizon steps, $k\in\mathbb{Z}_{1}^{n_{h}-1}$, can lead to overly
conservative behavior..

######  Remark 3.

In Theorem 2, a circular robot shape (47f) can be replaced by a tighter
polytopic boundary. In this case, the cost map introduced in the following
section may become orientation-dependent.

Data: Map parameters $N_{\epsilon}$, $r_{map}$, $x_{\mathcal{O},c}$,
$y_{\mathcal{O},c}$, occupancy map $\mathcal{O}$, current nominal state
$\bm{x}_{t}$, goal position $\bm{p}^{d}_{t}$, cost function parameters: $Q$,
$Q_{T}$, $R$, $\alpha_{cost}$, $\alpha_{shift}$, MPPI parameters
$\Sigma_{\bm{u}}$, $N_{sample}$,$\lambda$,$\alpha_{ISS}$, initial control
sequence $\\{\overline{\bm{u}}\\}_{0}^{n_{h}-1}$, and horizon $n_{h}$.

Result: $\\{\bm{x}^{*}_{i}\\}_{i=1}^{n_{h}}$,
$\\{\bm{u}^{*}_{i}\\}_{i=0}^{n_{h}-1}$, $\bm{u}_{send}$

Create $l_{map}+2N_{\epsilon}$ by $w_{map}+2N_{\epsilon}$ grid map
$\hat{\mathcal{C}}_{buffer}$.

Create $l_{map}+2N_{\epsilon}$ by $w_{map}+2N_{\epsilon}$ enlarged occupancy
map $\hat{\mathcal{O}}$ based on $\mathcal{O}$.

while  _task not completed_ do

for  _ $i=-N_{\epsilon},i<N_{\epsilon},i++$_ do

for  _$j=-N_{\epsilon},j <N_{\epsilon},j++$_ do

$C_{buffer}(N_{\epsilon}+i:N_{\epsilon}+l_{map}+i-1,N_{\epsilon}+j:N_{\epsilon}+w_{map}+j-1)+=\frac{\alpha_{shift}\hat{\mathcal{O}}_{i,j}^{s}}{100(\sqrt{i^{2}+j^{2}+1})}$.

end for

end for

$\mathcal{C}_{\epsilon}=\lceil\mathcal{C}_{buffer}(N_{\epsilon}:N_{\epsilon}+l_{map}-1,N_{\epsilon}:N_{\epsilon}+w_{map}-1),\overline{\mathcal{L}}_{track}\rceil$.

for  _ $k=0,k<N_{sample}-1,k++$_ do

Draw $\delta\bm{u}$ from $\mathcal{N}(\bm{0},\Sigma_{\bm{u}})$ $n_{h}$ times.
for  _$i=0,i <n_{h}-1,i++$_ do

$\bm{x}_{i+1}=\bm{x}_{i}+g(\bm{x}_{i})(\min(\max(\overline{\bm{u}}_{i}+\delta\bm{u}_{i},\bm{u}_{min}),\bm{u}_{max})\Delta
t$

end for

Evaluate the $k^{th}$ MPPI trajectory cost $\overline{\mathcal{L}}_{k}$ using
(48).

end for

Compute the optimal MPPI input sequence $\\{\bm{u}_{i}^{*}\\}_{i=0}^{n_{h}-1}$
using equations (25)-(26) with $\overline{\mathcal{L}}_{k}$ as the flow costs.

Compute best sampled state trajectory $\\{\bm{x}_{i}^{*}\\}_{i=1}^{n_{h}}$ and
use $\bm{x}_{1}^{*}$ and $\bm{x}_{0}$ to compute polar tracking error
$\hat{\bm{e}}$.

Use (52) to compute the input that will be sent to actuators.

For $i\\!=\\!0:n_{h}-2$: $\overline{\bm{u}}_{i}=\bm{u}^{*}_{i+1}$. Fill
$\overline{\bm{u}}^{*}_{n_{h}-1}$ using flatness relationship (5).

end while

Algorithm 2 Discrepancy-Aware MPPI

###  6.2 Discrepancy-Aware MPPI

The nonlinear program (47) can still be numerically challenging to solve,
especially when set $\mathcal{O}$ is represented in a grid map. Therefore, we
propose a discrepancy-aware cost map to encode the obstacle avoidance
constraint (47f). This map facilitates cost minimization, ensuring robust
obstacle avoidance amid model inaccuracy and vehicle geometry. Consider the
following augmented cost function which combines the reference tracking cost
(44) with a collision penalty,

$\overline{\mathcal{L}}=\mathcal{L}_{track}+\sum_{k=1}^{n_{h}}\mathcal{L}_{C}(\bm{p}_{k},\mathcal{C}_{\epsilon})+\alpha_{ISS}\bm{1}_{\|\bm{p}_{t+1}-\bm{p}_{t}\|\geq
r_{0}},$ (48)

where cost function
$\mathcal{L}_{C}:\mathbb{R}^{2}\times\mathbb{R}^{l_{map}\times
w_{map}}\to\mathbb{R}$ takes in a position $\bm{p}\in\mathbb{R}^{2}$ and a
$l_{map}\times w_{map}$ sized discrepancy aware cost map to produce a
collision cost. The parameter $\alpha_{ISS}\in\mathbb{R}_{>0}$ is chosen large
enough to force the planner to produce a trajectory that satisfies the
requirement $\|\bm{e}_{0}\|\leq r_{0}$ 777The term
$\alpha_{ISS}\bm{1}_{\|\bm{p}_{t+1}-\bm{p}_{t}\|\geq r_{0}}$ can be dropped if
sample trimming is incorporated where a MPPI sampled trajectory is omitted
when the inequality $\|\bm{p}_{t+1}-\bm{p}_{t}\|\geq r_{0}$ holds. The authors
used a replanning scheme instead of sample trimming in the experimental
validation section where if the inequality $\|\bm{p}_{1}-\bm{p}_{0}\|\geq
r_{0}$ holds, a new set of input discrepancies are sampled until the converse
is true. .

Using the discrete-time nominal model (47c), we apply MPPI to derive sub-
optimal trajectory and input sequences
($\\{\bm{x}^{*}_{t+k},\bm{u}^{*}_{t+k}\\}_{k=1}^{n_{h}}$) of optimization
problem (47) using the importance sampling law (25)-(26) and the proposed
discrepancy-aware cost function (48). To manage input constraints, we
introduce an element-wise clamping function
$\bm{u}_{clamp}=\max(\min(\bm{u}_{max},\bm{u}),\bm{u}_{min})$ [10], which does
not affect the MPPI algorithm’s convergence. The initial input sequence (warm
start) is obtained using the nominal dynamics and flatness properties as (5).
Algorithm 2 describes this process, including the construction of the cost map
$\mathcal{C}_{\epsilon}$.

###  6.3 Discrepancy-Aware Cost Map

Given that the nominal model (47c) focuses on the UGVs’ center of mass, we
account for the vehicle footprint via the smallest circumscribing circle with
radius, $r_{ego}$, see Fig. 6. We then enlarge each occupied grid by
$r_{ego}$. Additionally, to accommodate model mismatches, we further enlarge
the grids by $r_{\Delta T}$, creating a “collision buffer” around each
occupied grid. To encode this collision buffer in the nominal grid map, we
define $N_{\epsilon}\triangleq\lceil(r_{\Delta T}+r_{ego})/r_{map}\rceil$, a
positive integer that denotes the buffer’s grid size. The discrepancy-aware
cost map can be interpreted as a $N_{\epsilon}$ grid “inflation” of the
occupancy map to add a safety buffer between the ego vehicle and surrounding
obstacles (see Fig. 6 for an example of such inflation to account for the
unmodeled disturbances). The cost map construction is described in Algorithm
2, with added details in A.

The discrepancy-aware cost map, denoted as $\mathcal{C}_{\epsilon}$, satisfies
the following property:

$\mathcal{C}{\epsilon}(\bm{p})\geq\overline{\mathcal{L}}_{track}\quad\mbox{iff}\quad{C}(\bm{p},r_{\Delta
T}+r_{ego})\cap\overline{\mathcal{O}}\neq\emptyset$ (49)

for all positions $\bm{p}\in\mathbb{R}^{2}$ that place in the occupancy map.

Figure 6: A pictorial overview of the construction of the Discrepancy-Aware
Cost Map from occupancy map and collision buffer.

For MPPI cost evaluation, we map the position $\bm{p}=[x,y]^{T}$ to the
corresponding grid indices on $\mathcal{C}{\epsilon}$, calculating
$\mathcal{L}_{C}(\bm{p},\mathcal{C}{\epsilon})$ as the cost map entry at those
grid indices 888 Explicitly, we compute
$X_{\mathcal{C}}=\lfloor\frac{x}{r_{map}}+\frac{l_{map}}{2}\rfloor$ and
$Y_{\mathcal{C}}=\lfloor\frac{y}{r_{map}}+\frac{w_{map}}{2}\rfloor$ given
$\bm{p}_{k}$. Then, $\mathcal{L}_{C}(\bm{p}_{k},\mathcal{C}_{\epsilon})$ is
the $(X_{\mathcal{C}},Y_{\mathcal{C}})^{th}$ entries of
$\mathcal{C}_{\epsilon}$..

As an extension to Theorems 1 and 2, we have the following guarantees of the
proposed data-driven planning and control framework.

######  Corollary 1.

Suppose sufficient training samples, ${S}$, of the form (27), are available to
calculate $Z_{\epsilon},Z_{\epsilon}^{\perp}$ using Algorithm 1 and a user-
defined risk-level $\epsilon\in(0,1)$. Suppose there exists
$\tilde{\lambda}_{1}>0$ such that Theorem 1 holds. Under the same conditions
and assumptions as Theorem 2, a local minimum trajectory
$\\{\bm{x}^{*}_{t+k}\\}_{k=1}^{n_{h}}$ given by input sequence
$\\{\bm{u}^{*}_{t+k}\\}_{k=0}^{n_{h}-1}$ is obtained using the discrepancy-
aware MPPI algorithm, given by Algorithm 2. Applying control input

$\displaystyle\bm{u}_{t}=\bm{u}_{t}^{*}+\kappa_{ISS}(\bm{e}_{0})$ (50)

is sufficient to avoid the occupied grids in map $\mathcal{O}$ with a minimum
$(1-\epsilon)$ confidence, if the following conditions are satisfied:

  * 1.

The control inputs $\bm{u}_{t}$ from Eqn. (50) satisfy input constraint, i.e.
$\bm{u}_{t}\in\mathcal{D}_{\bm{u}}$.

  * 2.

The trajectory cost
$\overline{\mathcal{L}}(\\{\bm{x}^{*}_{k},\bm{u}^{*}_{k}\\}_{k=0}^{n_{h}-1})<\overline{\mathcal{L}}_{track}$.

###### Proof.

Based on the cost assignment of $\mathcal{C}_{\epsilon}$, if the total cost of
the planned trajectory
$\overline{\mathcal{L}}(\\{\bm{x}^{*}_{k},\bm{u}^{*}_{k}\\}_{k=0}^{n_{h}-1})$
is less than $\overline{\mathcal{L}}_{track}$, we can automatically guarantee
that
$\mathcal{L}_{C}(\bm{p}^{*}_{t+1},\mathcal{C}_{\epsilon})\leq\overline{\mathcal{L}}_{track}$,
i.e. collision safety with confidence $1-\epsilon$. Given the control policy
(50) also satisfies the control input limit, we can conclude that the flow of
the perturbed system (30) is within an expanding maximum track error tube of
radius $r_{t}$, calculated using (40), along the planned position trajectory
$\\{\bm{p}_{t+k}^{*}\\}_{k=0}^{1}$ with $(1-\epsilon)$ confidence. ∎

The theoretical guarantees of Corollary 1 serve as safety validation and
verification of the plan and also a safe controller to be synthesized, as
shown in the driver-assist application in Section 7.

##  7 Trajectory Tracking with Unknown Obstacles

Figure 7: Data-driven discrepancy identification results from configuration B
with $T_{lap}=\\{20,30,40,50\\}$ seconds. From the training set, we plot the
absolute unmatched drift against the input discrepancy norm, observing non-
negligible model discrepancy upper bounds. Outliers are observed due to
incorrect $\pi$ wrapping which can skew the empirical distribution formed by
the nonconformity scores as well as the conformal-driven discrepancies.

We first validated the proposed framework through high-speed trajectory
tracking experiments in the presence of stationary obstacles. These
experiments are conducted using four ground and vehicle configurations, as
depicted in Fig. 2.

The UGV is a Flipper Pro by Rover Robotics, with $r_{ego}=0.39~{}\mbox{m}$ in
the flipper down configuration and $r_{ego}=0.3~{}\mbox{m}$ in the flipper up
configuration. There are three stationary boxes in the test field acting as
obstacles, strategically placed to test the robot’s obstacle avoidance
capabilities (refer to Fig. 5 for layout details).

We construct a 2D occupancy map centered at $(0,0)$ with a $0.05~{}\mbox{m}$
grid size, updated at $2~{}\mbox{Hz}$, using a LiDAR sensor (VLP-16 by
Velodyne) mounted on the robot. We use a standard occupancy grid mapping
algorithm [60] to construct the occupancy map with robot poses provided by an
OptiTrack motion capture system. An input limit of
$v_{max}=[-2,2]~{}\mbox{m}/\mbox{s}$ and
$\omega_{max}=[-2,2]~{}\mbox{rad}/\mbox{s}$ is enforced. The dead zone is
selected to be $\rho_{min}=0.05\,\mbox{m}$, and the maximum position error is
set to $\rho_{max}=0.5\,\mbox{m}$, considering the input limits. All processes
are executed by an onboard AMD RyzenTM 9 6900HX CPU computer within a ROS 1
environment.

####  7.0.1 Training Details

Given that both terrains are flat, model discrepancies are primarily a
function of vehicle linear and angular velocities and input time delays. To
obtain training data that captures these discrepancies, we follow a “Figure-8”
trajectory, $x^{d}(t)=2.5\cos(2\pi t/T_{lap})$ and $y^{d}(t)=1.25\sin(4\pi
t/T_{lap})$, at four different desired lap timings without obstacles, denoted
as $T_{lap}$, for all four configurations. $T_{lap}$ takes values of $20$,
$30$, $40$, and $50$ seconds. We gathered approximately $5$ minutes of data at
each configuration, with a sampling rate of 20 Hz. A scatter plot of the
identified unmatched and matched disturbances from the training data is
displayed in Fig. 7 which showed non-negligible model discrepancies, mainly
arising from input delays and track slipping. Subsequently, we compute the
conformal-driven upper bounds and establish the maximum tracking deviation
radii, presented in Table 1 using sub-sample count of $L=3000$.

####  7.0.2 Choice of Parameters

The control algorithm operated at a frequency of $20$ Hz. The MPPI planning
horizon is $1.5$ seconds, with an input noise covariance matrix
$\sigma_{\bm{u}}=\mbox{diag}(0.2,0.2)$. The MPPI sample size was chosen to be
$N_{sample}=2000$. The MPPI costs in (44) were selected to be
$Q=\mbox{diag}(50,50)$, $R=\mbox{diag}(1,1)$, $Q_{f}=\mbox{diag}(200,200)$,
and $\alpha_{o}=\alpha_{ISS}=10000$. The MPPI inverse temperature parameter
$\lambda$ is chosen to be $0.1$. For the cost map, $\alpha_{shift}=0.1$,
$w_{map}=l_{map}=200$, and $r_{map}=0.05\,\mbox{m}$, with the map being
stationary and centered at the position $(0,0)$.

Figure 8: Experimental results of $30$-second laps “Figure-8” tracking in the
presence of stationary obstacles with test configurations A, B, C, and D. The
black pixels in the backgrounds for each configuration are the obstacle-
occupied grids from LiDAR measurements. The obstacles are placed to obstruct
the vehicle if not avoided. The unsafe probability $\epsilon$ ranges for the
four configurations are $\epsilon_{A1}=0.1760$, $\epsilon_{A2}=0.0029$,
$\epsilon_{A3}=0.0010$, $\epsilon_{A4}=0.0005$, $\epsilon_{B1}=0.1409$,
$\epsilon_{B2}=0.0016$, $\epsilon_{B3}=0.0013$, $\epsilon_{B4}=0.0011$,
$\epsilon_{C1}=0.1321$, $\epsilon_{C2}=0.0024$, $\epsilon_{C3}=0.0011$,
$\epsilon_{C4}=0.0009$, $\epsilon_{D1}=0.0031$, $\epsilon_{D2}=0.0016$,
$\epsilon_{D3}=0.0012$, and $\epsilon_{D1}=0.001$.

####  7.0.3 Controller Specifics

The effectiveness of our controller, as outlined in Theorem 1, is contingent
on the existence of the parameter, $\tilde{\lambda}_{1}$. However, the time
derivative of the Lyapunov function in (11) does not depend on the polar error
$\delta$, which implies that $\tilde{\lambda}_{1}$’s existence cannot be
assured. Focusing on the primary objective of tracking the desired positions
$\bm{p}^{d}(t)$, we consider the reduced polar coordinate error dynamics as
follows:

$\dot{\hat{\bm{e}}}=\frac{d}{dt}\begin{bmatrix}\rho\\\
\gamma\end{bmatrix}=\begin{bmatrix}-\cos(\gamma)&0\\\
\frac{\sin(\gamma)}{\rho}&-1\end{bmatrix}\delta\bm{u}=\hat{g}_{p}(\hat{\bm{e}})\delta\bm{u}.$
(51)

The control input of (51) is
$\bm{u}=\hat{\kappa}(\hat{\bm{e}})=[\hat{v},\hat{\omega}]^{T}$, where
$\hat{v}=k_{1}\rho\cos{\gamma}$ and
$\hat{\omega}=k_{2}\gamma+k_{1}\sin(\gamma)\cos(\gamma)$. With the Lyapunov
function $\hat{V}=\frac{1}{2}(\rho^{2}+\gamma^{2})$, the controlled system
(51) is exponentially stabilizing to $(0,0)$ within domain
$\mathcal{D}^{\rho}\setminus\mathcal{D}^{dz}\times\mathcal{D}^{\gamma}$ with
$\dot{\hat{V}}\leq-\alpha_{3}\|\hat{\bm{e}}\|$ and
$\alpha_{3}=\frac{k_{2}\rho_{dz}}{2\sqrt{(\pi/2)^{2}+\rho_{dz}}}$. Choosing
$k_{1}=0.3$ and $k_{2}=0.15$, the perturbed closed-loop system has a Lipschitz
constant $l_{\hat{V}}=\pi/2$, which is calculated over the input domain. For
tracking purposes, the convergence of $\hat{\bm{e}}$ to zero implies achieving
the desired output tracking, i.e., $\bm{p}\to\bm{p}^{*}$.

Applying Theorem 1 to the reduced polar coordinate error dynamics in (51), and
with our parameter choices, we obtain $\alpha_{1}=\alpha_{2}=0.5$. The class
$K_{\infty}$ function $\alpha_{3}(V)=0.0024V$ is affine. Most importantly,
there exists $\hat{\lambda}_{1}=1000$ which leads to
$\tilde{\alpha}_{3}(V)=-0.0038V$. With the augmented ancillary controller
$\hat{\kappa}_{ISS}=\hat{\kappa}(\hat{\bm{e}})-\frac{1}{\tilde{\lambda}_{1}}\hat{g}_{p}(\hat{\bm{e}})^{T}\hat{\bm{e}}$,
we compute the radii $r_{0}$ and $r_{\Delta T}$ for the 4 configurations
consolidated, see Table 1. The overall policy that is sent to the vehicle is:

$\bm{u}_{cmd}=\min(\max(\bm{u}_{0}^{*}+\hat{\kappa}_{ISS},\bm{u_{min}}),\bm{u}_{max}).$
(52)

Config $\epsilon$ | Param. | $0.001$ | $0.005$ | $0.01$
---|---|---|---|---
A | $Z_{\epsilon}\,|\,Z_{\epsilon}^{\perp}$ | $0.710\,|\,0.448$ | $0.423\,|\,0.025$ | $0.393\,|\,0.019$
$r_{0}\,|\,r_{\Delta T}$ | $0.252\,|\,0.255$ | $0.090\,|\,0.090$ | $0.077\,|\,0.077$
B | $Z_{\epsilon}\,|\,Z_{\epsilon}^{\perp}$ | $2.153\,|\,0.034$ | $0.419\,|\,0.030$ | $0.381\,|\,0.026$
$r_{0}\,|\,r_{\Delta T}$ | $2.318\,|\,2.320$ | $0.088\,|\,0.088$ | $0.073\,|\,0.073$
C | $Z_{\epsilon}\,|\,Z_{\epsilon}^{\perp}$ | $1.113\,|\,0.032$ | $0.413\,|\,0.027$ | $0.369\,|\,0.020$
$r_{0}|r_{\Delta T}$ | $0.619\,|\,0.620$ | $0.085\,|\,0.085$ | $0.068\,|\,0.068$
D | $Z_{\epsilon}\,|\,Z_{\epsilon}^{\perp}$ | $1.878\,|\,0.095$ | $0.429\,|\,0.032$ | $0.401\,|\,0.031$
$r_{0}\,|\,r_{\Delta T}$ | $1.763\,|\,1.768$ | $0.092\,|\,0.092$ | $0.080\,|\,0.081$

Table 1: Summary of the offline conformal discrepancy training results and the
augmented controller tracking guarantees where autonomous trajectory tracking
is performed without obstacles in the four configurations. The training
$1-\epsilon$ confidence upper bounds for $\|\bm{d}_{\bm{u}}\|$ and
$|d{d}_{\perp}|$ is provided at three $\epsilon$ levels using Algorithm 1.
Under the augmented control policy ${\kappa}_{ISS}$, we tabulated $r_{0}$ and
$r_{\Delta T}$ with the choices of $\epsilon$ levels. Note, the identified
radii $r_{0}$, $r_{\Delta t}$ are in meters.

####  7.0.4 Results

The discrepancy-aware MPPI planner is validated by tracking the same “Figure
8” trajectory used for collecting training data, denoted as
$\bm{p}^{d}=(x^{d}(t),y^{d}(t))$, with a lap time of
$T_{lap}=30~{}\mbox{sec}$999Despite the validation desired trajectory’s
geometry matches with the training set, the vehicle must deviate from the
reference trajectory to avoid the stationary obstacles, covering untrained
positions. The experimental setup can be found in Fig. 5. As highlighted in
the supplementary video [61], the nominal MPPI planner combined with the
nominal controller without accounting model mismatches, failed to ensure
safety, resulting in a collision with the stationary crates. In contrast, our
proposed framework completed the trajectory tracking tasks across all four
vehicle-ground configurations, effectively handling model mismatches while
providing a verifiable safe traversal confidence level. The experimental
results are summarized in Fig. 8 for the four vehicle-ground configurations of
Fig. 2.

A key feature of our approach is the augmentation of the cost map on a grid
basis. We associate each increment in $N_{\epsilon}$ with a specific range of
$\epsilon$ values, demonstrating the tunability of our framework. It was
observed that lower values of $\epsilon$ (approaching zero) induce a
conservative obstacle-avoiding behavior, akin to traditional robust control
methods. Conversely, as $\epsilon$ nears one, the risk-neutral case, the
vehicle tracks closely with the reference path, shows improvements in tracking
performance at the cost of safety confidence.

During our analysis, particularly with configurations B and D of Fig. 2, large
model discrepancies for $\epsilon=0.001$ are observed. These discrepancies
were predominantly found in datasets characterized by lower lap speeds,
suggesting a correlation with specific state or input conditions. This
observation signals the possibilities of a state and input-dependent
discrepancy discussed in [62], which could potentially provide a more accurate
safety buffer compared to our upper bounds $Z_{\epsilon}$ and
$Z_{\epsilon}^{\perp}$.

###  7.1 Driver Assist for Collision Avoidance

Figure 9: Experimental results of using the proposed framework performing
collision avoidance assistance for human drivers. Based on human-provided
joystick commands and a user-chosen risk tolerance $\epsilon$, the collision
risk of joystick trajectory based on current state feedback is evaluated on
the proposed discrepancy-aware cost map. If the collision is projected based
on the evaluated cost, the proposed planning and control framework will
activate to provide overriding safe commands. Sub-figure (a) is a diagram that
summarizes the driver-assist logic flows. Sub-figures (b1) and (b2) illustrate
the scenario for safe joystick inputs where no collision assist is inactive.
Sub-figures (c1) and (c2) illustrate the scenario where the projected joystick
trajectory is unsafe and where the collision assist program is active to
provide optimal and safe commands. Sub-figures (d1)-(d6) are the six test
subjects’ trajectories in manual mode and driver assist mode.

####  7.1.1 Human-in-the-Loop Setup

This section explores the application of our proposed framework in driver-
assist scenarios. A driver remotely operates a UGV robot via a joystick. The
driver controls the vehicle’s linear velocity in the body frame’s $x$ axis
($v_{joy}$) and angular velocity in the $z$ axis ($\omega_{joy}$), the same as
the input commands in (1). The driver assist program aims to follow the
joystick commands as closely as possible while avoiding obstacles up to the
specified risk tolerance.

Drawing from Hugemann’s study on driver reaction times in road traffic [63],
we anticipate a, rather conservative, human reaction time of $1.5$ seconds for
collision avoidance. We interpret this $1.5$ seconds reaction time as a $1.5$
second ZOH to the most recent joystick commands. Specifically, we replace the
desired reference trajectory with the joystick trajectory
$\\{\bm{x}_{t+k}\\}_{k=1}^{n_{h}}$, formulated as

$\bm{x}_{t+k}^{joy}=\bm{x}_{t}+\sum_{i=0}^{k}g(\bm{x}_{t+i})\begin{bmatrix}v_{joy}\\\
\omega_{joy}\end{bmatrix}\Delta T,$ (53)

where $k\in\mathbb{Z}_{1}^{n_{h}}$. To ensure maximum teleoperation control in
collision-free environments, we only provide trajectory correction when a
collision is imminent based on the joystick trajectory.

In our experiment, human subjects drive the UGV in configuration A without a
direct view of the test course. The test drivers rely solely on a 2D occupancy
map for navigating among obstacles, matching the perception capability of the
framework. The human drivers are tasked to finish the narrow test course
without collision as quickly as possible.

Note, the same training set presented in section 7.0.1 is used in the driver
assist application despite the vehicle trajectory does not match the training
Figure 8 trajectory to highlight the learned upper bounds $Z_{\epsilon}$ and
$Z_{\epsilon}^{\perp}$ is training trajectory independent.

####  7.1.2 Driver-Assist Implementation

Using the training result tabulated in table 1, we construct the discrepancy-
aware cost map with a risk value $\epsilon$ ranges in $(0.0029,0.0010)$. We
determine a joystick trajectory cost:

$\overline{\mathcal{L}}_{joy}=\sum_{k=1}^{n_{h}}\alpha_{joy}\mathcal{L}_{\mathcal{C}}(\bm{p}_{t+k}^{joy},\mathcal{C}_{\epsilon}),$
(54)

where $\alpha_{joy}\in\mathbb{R}_{>0}$ is an adjustable parameter with default
value of $1.0$. The cost-to-go and input cost are set to zero, considering the
joystick trajectory itself is the desired reference. If
$\overline{\mathcal{L}}_{joy}$ is less than $\overline{\mathcal{L}}_{track}$,
the joystick trajectory is deemed safe with a confidence level of $1-\epsilon$
for the immediate $1.5$ seconds. Conversely, if
$\overline{\mathcal{L}}_{joy}\geq\overline{\mathcal{L}}_{track}$, there is a
potential collision, requiring a safety override.

The proposed framework implements the safety override. We initialized the MPPI
algorithm using the joystick input as a warm start, with input perturbation
covariance set as a diagonal matrix of $0.25~{}m/s$ and $~{}0.25rad/s$ for
linear and angular velocities. Since the maximum joystick linear and angular
speeds are $2~{}m/s$ and $2~{}rad/s$, a zero-speed (stopping) command cannot
be guaranteed to be sampled. As a solution, we also add turn-in-place (TIP)
motion primitives into the flow sampling set and MPPI input aggregation, in
addition to the joystick trajectory. Specifically, the joystick turning
command with $0$ linear velocity,
$\\{\bm{u}_{k}^{TIP}\\}_{k=0}^{k=n_{h}-1}=[0,\omega_{joy}]^{T}$, allow the
driver to perform heading adjustments freely, but not the vehicle positions
because of potential collision risks in the current heading direction.

With a total MPPI sample size of $N_{sample}=5000$ and inverse temperature
$\lambda=0.05$, we allocated and aggregated $80\%$ of sampled input
discrepancies to the joystick input sequences and the remaining samples to the
TIP motion primitives. Following Algorithm (2) to compute the associated costs
of each sampled flow, we obtain the collision-assist safety override
$\bm{u}_{cmd}^{safe}$. 101010We always use the joystick trajectory as a warm
start without using the previous MPPI solution. Such engineering decision is
made to ensure maximum adherence to the driver’s command which can be
discontinuous and far from the previous projected joystick trajectory. A
detailed diagram of the collision-assist program is presented in Fig. (9).

We select parameter $\alpha_{joy}$ to be $1/{k^{2}}$, inversely proportional
to the prediction horizon squared. As the collision-assistance program
recalculates at $20$ Hz, this cost decay over future horizon steps is chosen
to reduce planner conservatism.

####  7.1.3 Results and Discussion

We conducted an experimental comparison with six human drivers navigating a
narrow pathway, with various experience levels with joystick operation. Each
test subject drives the same test course twice, once with and once without the
proposed collision assistive program. The order of the testing is randomized
to minimize any bias due to the driver’s familiarity with the test course. The
test results are tabulated in Table 2, with comparative drive trajectories
depicted in Fig. 9.

Subj. | Seq. |  $\\#$ C (M) |  $T_{t}$ (M) |  $\\#$ C (CA) |  $T_{t}$ (CA)
---|---|---|---|---|---
1 | M/DA | 2 | 63.7 | 0 | 38.8
2 | M/DA | 0 | 50.7 | 1 | 40.3
3 | DA/M | 0 | 41.5 | 0 | 44.8
4 | M/DA | 7 | 107.5 | 3 | 63.6
5 | DA/M | 0 | 45.6 | 0 | 30.1
6 | DA/M | 1 | 35.6 | 1 | 28.3

Table 2: Summary of the diver assist program with 6 different human drivers.
Each subject is assigned to attempt to complete the task with either manual
mode (M) or the collision assistance mode (CA). The mode experience order is
indicated in the second column. The number of collisions ($\\#C$) during the
test drives is recorded and the test drive duration $T_{t}$.

Based on the tabulated results, we can see the proposed collision-assistive
program showed minor improvements for rather inexperienced drivers (such as
drivers 1 and 4) who faced challenges during their initial manual run. Despite
the driving sequence being assigned randomly, a consistent reduction in
driving time was observed on their second pass, likely attributable to the
accumulation of track and vehicle knowledge. Notably, subjects who initially
experienced manual driving followed by the driver-assist program exhibited
percent drive time improvements of $39.1\%$, $20.5\%$, and $40.8\%$.
Conversely, subjects who first used the assistive program and then switched to
manual mode showed improvements of $-8.0\%$, $34.0\%$, and $20.5\%$. This
variance in performance improvement may be attributed to enhanced driver
confidence in collision avoidance due to the assistive program.

The oral feedback from test subjects on the driver-assist program was mixed.
Positive remarks primarily center around the program’s effectiveness and the
ability to provide collision-avoiding heading adjustments automatically.
However, several drivers noted counterintuitive instances where the vehicle,
under driver-assist control, executed minor reverse maneuvers despite
receiving forward velocity commands. This behavior is the result of the model
mismatches, where the nominal model predicts a trajectory leading into high-
risk or collision-prone areas, prompting the program to revert to a safer
region. Furthermore, integrating user-preference-based cost tuning could
better tailor individual drivers’ preferences in balancing between reference
tracking and collision safety, as suggested in recent studies [64, 65]. It is
important to note that due to the limited number of participants, we refrain
from drawing statistically significant conclusions from this study.

###  7.2 Limitation and Future Works

The current limitations of the proposed framework include potential safety
violations in the importance sampling aggregation law [39], a tendency towards
planning overly conservative trajectories, and vulnerability to training
outliers in model discrepancies. Based on the choice of the inverse
temperature $\lambda$, $\lambda\to\infty$ plans smoother trajectories with
equal weighting, and $\lambda\to 0$ equates to sample rejection that could
lead to instability [52]. However, sampled input aggregation as
$\lambda\to\infty$ could lead to an unsafe aggregated trajectory where safe
and unsafe trajectories are less distinguished. Nevertheless, $\lambda\to 0$
prioritizes picking safe trajectories but may lead to overly conservative and
chattered trajectories from randomly sampled input sequences.

On the discrepancy identification front, conformal prediction relies on
empirical distribution quantiles instead of concentration inequalities,
allowing it to be more sample-efficient. However, in the presence of a large
number of samples, potential outliers, and small risk tolerance, the
identified $Z_{\epsilon}$ and $Z_{\epsilon}^{\perp}$ can be falsely large,
yielding overly conservative results. Sufficient data set prepossessing might
be required to remove outliers.

Looking ahead, our future work aims to refine the offline conformal-driven
discrepancy upper bound analysis into an online algorithm. We are also
exploring adaptive conformal prediction methods, akin to the one detailed in
[51], to actively detect distribution shifts from the training sets due to
terrain or environmental changes. Moreover, a higher fidelity nominal model
such as a learning-based model can be used to enhance tracking performance.
Another future research involves the theoretical analysis of the discrepancy-
aware MPPI in terms of optimality and recursive feasibility. The authors are
also investigating the possibilities of rejecting unsafe MPPI trajectory
samples, incorporating motion primitives, and incorporating generalized state
constraints beyond obstacle avoidance constraints.

##  8 Conclusion

In summary, this manuscript provides a novel multi-layered framework that is
designed to provide safety-critical autonomy in the presence of obstacles and
model discrepancies. The discrepancies include poorly modeled terrain
interactions, system delays, and simplified dynamic models. The framework’s
core strategy involves data-driven discrepancy identification, extracting both
matched and unmatched model residuals from offline data, with minimal
assumptions. These identified discrepancies are then used to augment the
vehicle’s ancillary controller, offering stability assurances for the closed-
loop system. We then complete the framework with a discrepancy-aware MPPI
planner that generates (sub)optimal and safe reference tracking paths, taking
into account imperfection in actual trajectory tracking due to model
discrepancies.

Our proposed framework is theoretically supported throughout its construction.
By deducing the maximum tracking error resulting from matched and unmatched
model discrepancies, we ensure safety and robustness by assessing the
interactions between the planner and controller layers. We also validated the
proposed framework through extensive hardware experiments, demonstrating its
effectiveness in trajectory tracking in cluttered environments. Additionally,
we have successfully adapted the framework as a driver-assist program,
providing optimal, safe assistive commands in potential collision scenarios.

## Acknowledgements

This work was supported by DARPA under the Learning and Introspective Control
(LINC) program and by the Technology Innovation Institute (TII) through a
grant to the Center for Autonomous Systems and Technologies.

The authors thank Anushri Dixit and Thomas Tomar for their help in configuring
the Flipper Pro. We thank Kejun Li, William Welch, Victor Zendejas Lopez,
Mandralis Ioannis, and Lizhi Yang for helping with the experiments.

## References

  * [1] S. Bonadies, A. Lefcourt, S. A. Gadsden, A survey of unmanned ground vehicles with applications to agricultural and environmental sensing, in: Autonomous air and ground sensing systems for agricultural optimization and phenotyping, Vol. 9866, SPIE, 2016, pp. 142–155.
  * [2] K. Ebadi, Y. Chang, M. Palieri, A. Stephens, A. Hatteland, E. Heiden, A. Thakur, N. Funabiki, B. Morrell, S. Wood, L. Carlone, A.-a. Agha-mohammadi, Lamp: Large-scale autonomous mapping and positioning for exploration of perceptually-degraded subterranean environments, in: 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020, pp. 80–86.  doi:10.1109/ICRA40945.2020.9197082.
  * [3] Y. Wu, Y. Ding, S. Ding, Y. Savaria, M. Li, Autonomous last-mile delivery based on the cooperation of multiple heterogeneous unmanned ground vehicles, Mathematical Problems in Engineering 2021 (2021) 1–15.
  * [4] J. Thangavelautham, A. Chandra, E. Jensen, Autonomous robot teams for lunar mining base construction and operation, in: 2020 IEEE Aerospace Conference, 2020, pp. 1–16.  doi:10.1109/AERO47225.2020.9172811.
  * [5] P. Akella, A. Dixit, M. Ahmadi, J. W. Burdick, A. D. Ames, Sample-based bounds for coherent risk measures: Applications to policy synthesis and verification, arXiv:2204.09833 (2022).
  * [6] M. Saveriano, Y. Yin, P. Falco, D. Lee, Data-efficient control policy search using residual dynamics learning, in: 2017 Int. Conference on Intelligent Robots and Systems (IROS), 2017, pp. 4709–4715.
  * [7] A. Dixit, D. D. Fan, K. Otsu, S. Dey, A.-A. Agha-Mohammadi, J. W. Burdick, Step: Stochastic traversability evaluation and planning for risk-aware off-road navigation; results from the darpa subterranean challenge, arXiv preprint arXiv:2303.01614 (2023).
  * [8] A. Bouman, M. F. Ginting, N. Alatur, M. Palieri, D. D. Fan, T. Touma, T. Pailevanian, S.-K. Kim, K. Otsu, J. Burdick, A. akbar Agha-mohammadi, Autonomous spot: Long-range autonomous exploration of extreme environments with legged locomotion (2020).  arXiv:2010.09259.
  * [9] L. Gan, J. W. Grizzle, R. M. Eustice, M. Ghaffari, Energy-based legged robots terrain traversability modeling via deep inverse reinforcement learning, IEEE Robotics and Automation Letters 7 (4) (2022) 8807–8814.  doi:10.1109/LRA.2022.3188100.
  * [10] G. Williams, B. Goldfain, P. Drews, K. Saigol, J. M. Rehg, E. A. Theodorou, Robust sampling based model predictive control with sparse objective information., in: Robotics: Science and Systems, Vol. 14, 2018, p. 2018.
  * [11] G. Williams, N. Wagener, B. Goldfain, P. Drews, J. M. Rehg, B. Boots, E. A. Theodorou, Information theoretic mpc for model-based reinforcement learning, in: 2017 IEEE International Conference on Robotics and Automation (ICRA), 2017, pp. 1714–1721.  doi:10.1109/ICRA.2017.7989202.
  * [12] R. Sinha, J. Harrison, S. M. Richards, M. Pavone, Adaptive robust model predictive control with matched and unmatched uncertainty, in: 2022 American Control Conference (ACC), IEEE, 2022, pp. 906–913.
  * [13] W.-J. Cao, J.-X. Xu, Nonlinear integral-type sliding surface for both matched and unmatched uncertain systems, IEEE Transactions on Automatic Control 49 (8) (2004) 1355–1360.  doi:10.1109/TAC.2004.832658.
  * [14] J. Pravitra, K. A. Ackerman, C. Cao, N. Hovakimyan, E. A. Theodorou, $l_{1}$-adaptive mppi architecture for robust and agile control of multirotors, in: 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE, 2020, pp. 7661–7666.
  * [15] L. Lindemann, M. Cleaveland, G. Shim, G. J. Pappas, Safe planning in dynamic environments using conformal prediction, IEEE Robotics and Automation Letters 8 (8) (2023) 5116–5123.  doi:10.1109/LRA.2023.3292071.
  * [16] A. Alan, A. J. Taylor, C. R. He, G. Orosz, A. D. Ames, Safe controller synthesis with tunable input-to-state safe control barrier functions, IEEE Control Systems Letters 6 (2021) 908–913.
  * [17] N. Csomay-Shanklin, A. Taylor, U. Rosolia, A. Ames, Multi-rate planning and control of uncertain nonlinear systems: Model predictive control and control lyapunov functions (03 2022).
  * [18] A. Majumdar, M. Pavone, How should a robot assess risk? towards an axiomatic theory of risk in robotics, in: Robotics Research, Springer, 2020, pp. 75–84.
  * [19] A. Dixit, M. Ahmadi, J. W. Burdick, Risk-sensitive motion planning using entropic value-at-risk, in: 2021 European Control Conference (ECC), IEEE, 2021, pp. 1726–1732.
  * [20] S. Cakmak, R. Astudillo Marban, P. Frazier, E. Zhou, Bayesian optimization of risk measures, Advances in Neural Information Processing Systems 33 (2020) 20130–20141.
  * [21] A. Makarova, I. Usmanova, I. Bogunovic, A. Krause, Risk-averse heteroscedastic bayesian optimization, Advances in Neural Information Processing Systems 34 (2021) 17235–17245.
  * [22] M. O’Connell, G. Shi, X. Shi, K. Azizzadenesheli, A. Anandkumar, Y. Yue, S.-J. Chung, Neural-fly enables rapid learning for agile flight in strong winds, Science Robotics 7 (66) (2022).
  * [23] A. Bemporad, M. Morari, Robust model predictive control: A survey, in: Robustness in identification and control, Springer, 2007, pp. 207–226.
  * [24] B. T. Lopez, J.-J. E. Slotine, J. P. How, Dynamic tube mpc for nonlinear systems, in: 2019 American Control Conference (ACC), IEEE, 2019, pp. 1655–1662.
  * [25] Y. K. Nakka, S.-J. Chung, Trajectory optimization of chance-constrained nonlinear stochastic systems for motion planning under uncertainty, IEEE Transactions on Robotics 39 (1) (2022) 203–222.
  * [26] H. Zhu, J. Alonso-Mora, Chance-constrained collision avoidance for mavs in dynamic environments, IEEE Robotics and Automation Letters 4 (2) (2019) 776–783.  doi:10.1109/LRA.2019.2893494.
  * [27] L. Blackmore, M. Ono, A. Bektassov, B. C. Williams, A probabilistic particle-control approximation of chance-constrained stochastic predictive control, IEEE Transactions on Robotics 26 (3) (2010) 502–517.  doi:10.1109/TRO.2010.2044948.
  * [28] L. Blackmore, M. Ono, B. C. Williams, Chance-constrained optimal path planning with obstacles, IEEE Transactions on Robotics 27 (6) (2011) 1080–1094.  doi:10.1109/TRO.2011.2161160.
  * [29] G. C. Calafiore, L. Fagiano, Stochastic model predictive control of lpv systems via scenario optimization, Automatica 49 (6) (2013) 1861–1866.
  * [30] M. S. Gandhi, B. Vlahov, J. Gibson, G. Williams, E. A. Theodorou, Robust model predictive path integral control: Analysis and performance guarantees, IEEE Robotics and Automation Letters 6 (2) (2021) 1423–1430.  doi:10.1109/LRA.2021.3057563.
  * [31] S. Thrun, Probabilistic robotics, Commun. ACM 45 (3) (2002) 52–57.  doi:10.1145/504729.504754.
URL https://doi.org/10.1145/504729.504754

  * [32] S. Thrun, A. Bücken, Integrating grid-based and topological maps for mobile robot navigation, in: Proceedings of the national conference on artificial intelligence, 1996, pp. 944–951.
  * [33] E. G. Tsardoulias, A. Iliakopoulou, A. Kargakos, L. Petrou, A review of global path planning methods for occupancy grid maps regardless of obstacle density, Journal of Intelligent & Robotic Systems 84 (2016) 829–858.
  * [34] H. Oleynikova, A. Millane, Z. Taylor, E. Galceran, J. I. Nieto, R. Y. Siegwart, Signed distance fields: A natural representation for both mapping and planning, 2016.
URL https://api.semanticscholar.org/CorpusID:28083959

  * [35] G. S. Camps, R. Dyro, M. Pavone, M. Schwager, Learning deep sdf maps online for robot navigation and exploration (2022).  arXiv:2207.10782.
  * [36] D. D. Fan, A.-a. Agha-mohammadi, E. A. Theodorou, Learning risk-aware costmaps for traversability in challenging environments, IEEE Robotics and Automation Letters 7 (1) (2022) 279–286.  doi:10.1109/LRA.2021.3125047.
  * [37] X. Cai, S. Ancha, L. Sharma, P. R. Osteen, B. Bucher, S. Phillips, J. Wang, M. Everett, N. Roy, J. P. How, Evora: Deep evidential traversability learning for risk-aware off-road autonomy (2023).  arXiv:2311.06234.
  * [38] L. Sharma, M. Everett, D. Lee, X. Cai, P. Osteen, J. How, Ramp: A risk-aware mapping and planning pipeline for fast off-road ground robot navigation (09 2022).  doi:10.48550/arXiv.2210.06605.
  * [39] J. Yin, C. Dawson, C. Fan, P. Tsiotras, Shield model predictive path integral: A computationally efficient robust mpc approach using control barrier functions (2023).  arXiv:2302.11719.
  * [40] J. Clark, D. Amodei, Faulty reward functions in the wild.
URL https://openai.com/research/faulty-reward-functions

  * [41] J. Frey, M. Mattamala, N. Chebrolu, C. Cadena, M. Fallon, M. Hutter, Fast traversability estimation for wild visual navigation, arXiv preprint arXiv:2305.08510 (2023).
  * [42] G. Klancar, A. Zdesar, S. Blazic, I. Skrjanc, Wheeled mobile robotics: from fundamentals towards autonomous systems, Butterworth-Heinemann, 2017.
  * [43] M. Aicardi, G. Casalino, A. Bicchi, A. Balestrino, Closed loop steering of unicycle like vehicles via lyapunov techniques, IEEE Robotics and Automation Magazine 2 (1) (1995) 27–35.
  * [44] I. S. Mohamed, G. Allibert, P. Martinet, Model predictive path integral control framework for partially observable navigation: A quadrotor case study, in: International Conference on Control, Automation, Robotics and Vision, 2020, pp. 196–203.  doi:10.1109/ICARCV50220.2020.9305363.
  * [45] B. Lindqvist, S. S. Mansouri, A.-a. Agha-mohammadi, G. Nikolakopoulos, Nonlinear mpc for collision avoidance and control of uavs with dynamic obstacles, IEEE robotics and automation letters 5 (4) (2020) 6001–6008.
  * [46] M. Diehl, J. Bjornberg, Robust dynamic programming for min-max model predictive control of constrained uncertain systems, IEEE Transactions on Automatic Control 49 (12) (2004) 2253–2257.
  * [47] A. De Luca, G. Oriolo, M. Vendittelli, Control of Wheeled Mobile Robots: An Experimental Overview, Springer Berlin Heidelberg, Berlin, Heidelberg, 2001, pp. 181–226.
  * [48] H. K. Khalil, Nonlinear systems, Prentice Hall, 2002.
  * [49] N. Chakraborty, A. Ghosal, Kinematics of wheeled mobile robots on uneven terrain, Mechanism and Machine Theory 39 (12) (2004) 1273–1287, 11th National Conference on Machines and Mechanisms (NaCoMM-2003).  doi:https://doi.org/10.1016/j.mechmachtheory.2004.05.016.
URL https://www.sciencedirect.com/science/article/pii/S0094114X04001478

  * [50] V. Vovk, A. Gammerman, G. Shafer, Algorithmic Learning in a Random World, Springer-Verlag, Berlin, Heidelberg, 2005.
  * [51] A. Dixit, L. Lindemann, S. X. Wei, M. Cleaveland, G. J. Pappas, J. W. Burdick, Adaptive conformal prediction for motion planning among dynamic agents, in: Learning for Dynamics and Control Conference, PMLR, 2023, pp. 300–314.
  * [52] G. Williams, P. Drews, B. Goldfain, J. M. Rehg, E. A. Theodorou, Information-theoretic model predictive control: Theory and applications to autonomous driving, IEEE Transactions on Robotics 34 (6) (2018) 1603–1622.  doi:10.1109/TRO.2018.2865891.
  * [53] A. N. Angelopoulos, S. Bates, A gentle introduction to conformal prediction and distribution-free uncertainty quantification, arXiv preprint arXiv:2107.07511 (2021).
  * [54] R. J. Tibshirani, R. Foygel Barber, E. Candes, A. Ramdas, Conformal prediction under covariate shift, Advances in neural information processing systems 32 (2019).
  * [55] M. Sadinle, J. Lei, L. Wasserman, Least ambiguous set-valued classifiers with bounded error levels, Journal of the American Statistical Association 114 (525) (2019) 223–234.
  * [56] G. Williams, P. Drews, B. Goldfain, J. M. Rehg, E. A. Theodorou, Aggressive driving with model predictive path integral control, in: 2016 IEEE International Conference on Robotics and Automation (ICRA), 2016, pp. 1433–1440.  doi:10.1109/ICRA.2016.7487277.
  * [57] A. Z. Ren, A. Dixit, A. Bodrova, S. Singh, S. Tu, N. Brown, P. Xu, L. Takayama, F. Xia, J. Varley, Z. Xu, D. Sadigh, A. Zeng, A. Majumdar, Robots that ask for help: Uncertainty alignment for large language model planners (2023).  arXiv:2307.01928.
  * [58] T. H. Gronwall, Note on the derivatives with respect to a parameter of the solutions of a system of differential equations, Annals of Mathematics 20 (4) (1919) 292–296.
  * [59] G. Bastos Jr, E. Franco, Energy shaping dynamic tube-mpc for underactuated mechanical systems, Nonlinear Dynamics 106 (1) (2021) 359–380.
  * [60] S. Thrun, Probabilistic robotics, Communications of the ACM 45 (3) (2002) 52–57.
  * [61] Supplementary video.
URL https://youtu.be/0tZagFMfodI

  * [62] P. Akella, S. X. Wei, J. W. Burdick, A. Ames, Learning disturbances online for risk-aware control: Risk-aware flight with less than one minute of data, in: Proceedings of The 5th Annual Learning for Dynamics and Control Conference, Vol. 211 of Proceedings of Machine Learning Research, PMLR, 2023, pp. 665–678.
  * [63] W. Hugemann, Driver reaction times in road traffic, 2002.
URL https://api.semanticscholar.org/CorpusID:58933066

  * [64] R. Cosner, M. Tucker, A. Taylor, K. Li, T. Molnar, W. Ubelacker, A. Alan, G. Orosz, Y. Yue, A. Ames, Safety-aware preference-based learning for safety-critical control, in: R. Firoozi, N. Mehr, E. Yel, R. Antonova, J. Bohg, M. Schwager, M. Kochenderfer (Eds.), Proceedings of The 4th Annual Learning for Dynamics and Control Conference, Vol. 168 of Proceedings of Machine Learning Research, PMLR, 2022, pp. 1020–1033.
  * [65] M. Tucker, K. Li, Y. Yue, A. D. Ames, Polar: Preference optimization and learning algorithms for robotics (2022).  arXiv:2208.04404.

##  Appendix A Discrepancy-Aware Cost Map Construction Details

This section details the process of constructing a discrepancy-aware cost map
given an occupancy map $\mathcal{O}$. For every occupied grid, its neighboring
grids within a distance of $(r_{\Delta T}+r_{ego})$, referred to as the safety
buffer, will also be considered as occupied grids to ensure robust obstacle
avoidance given model inaccuracy and vehicle geometry.

An enlarged grid map
$\hat{\mathcal{O}}\in\mathbb{R}^{l_{map}+2N_{\epsilon}\times
w_{map}+2N_{\epsilon}}$ is initialized, and the nominal map is shifted and
aggregated to form the collision buffer grid map (55) and visualized in Figure
1. Specifically, We shift the nominal occupancy map by $N_{\epsilon}$ grids in
both column-wise and row-wise positive and negative directions, as described
in Algorithm 2. In total, there are $4N_{\epsilon}+2$ shifted grid maps which
are grid-wise aggregated to obtain the grid map with collision buffer,

$\hat{\mathcal{O}}_{buffer}\triangleq\frac{1}{4N_{\epsilon}+2}\sum_{i=-N_{\epsilon}}^{N_{\epsilon}}\sum_{j=-N_{\epsilon}}^{N_{\epsilon}}\frac{\hat{\mathcal{O}}^{s}_{i,j}}{100},$
(55)

where we use
$\hat{\mathcal{O}}^{s}_{i,j}\triangleq\hat{\mathcal{O}}(N_{\epsilon}+i:N_{\epsilon}+l_{map}+i-1,N_{\epsilon}+j:N_{\epsilon}+w_{map}+j-1)$
as the grid shifting operation. The normalization factor $100$ is applied on
the enlarged occupancy maps to obtain a value within $[0,1]$.

The discrepancy-aware cost map $\mathcal{C}_{\epsilon}$ is constructed by
applying a tunable cost multiplier $\alpha_{shift}/N_{shift,\epsilon}$ to this
enlarged occupancy map $\hat{\mathcal{O}}_{buffer}$ The multiplier
$\alpha_{shift}/N_{shift,\epsilon}$ to the $N_{\epsilon}$-shifted occupancy
maps is a design parameter where $\alpha_{shift}\in\mathbb{R}_{>0}$ is a cost
multiplier similar to $\alpha_{ISS}$ in (48) and the positive integer
$N_{shift}\in\\{1,\ldots,N_{\epsilon}\\}$ serves as a cost decay. This cost
adjustment accounts for both conservative obstacle avoidance and model
mismatches, ensuring that the true and uncertain system flow remains
collision-free given the risk tolerance. The final cost map balances obstacle
collision penalties and tracking costs (Algorithm 2). The design parameter
$\alpha_{shift}/N_{\epsilon}$ needs to be lower bounded by the maximum
tracking cost $\overline{L}_{track}$ to penalize unsafe trajectories. A larger
$\alpha_{shift}$ leads to more conservative obstacle avoidance behavior, and a
smaller $\alpha_{shift}$ reduces the cost associated with the avoidance of the
safe buffer due to model mismatches. The discrepancy-aware cost map, denoted
as $\mathcal{C}_{\epsilon}$ (See Algorithm 2), is the centering $l_{map}$ and
$w_{map}$ grids of the following enlarged cost map:

$\mathcal{C}_{buffer}\triangleq\sum_{i=-N_{\epsilon}}^{N_{\epsilon}}\sum_{j=-N_{\epsilon}}^{N_{\epsilon}}\alpha_{shift}/\underbrace{\sqrt{i^{2}+j^{2}}}_{\triangleq
N_{shift,\epsilon}}\hat{\mathcal{O}}^{s}_{i,j}.$ (56)

Lastly, we apply a maximum cost map threshold
$\mathcal{C}_{\epsilon}=\min(\mathcal{C}_{\epsilon},\overline{L}_{track})$
with the maximum tracking cost grid-wise to even the cost penalty for obstacle
collision.