# On The Evaluation of
Collision Probability along a Path ††thanks: This work was supported by the EU
Project DARKO, Grant ID 101017274.

Lorenzo Paiola1,2, Giorgio Grioli2, Antonio Bicchi1,2 1Soft Robotics for Human
Cooperation and Rehabilitation, Istituto Italiano di Tecnologia, Genova 16163,
Italy (lorenzo.paiola@iit.it).2Department of Information Engineering and
Centro di Ricerca “Enrico Piaggio”, University of Pisa, 56122, Pisa, Italy.

###### Abstract

Characterizing the risk of operations is a fundamental requirement in
robotics, and a crucial ingredient of safe planning. The problem is
multifaceted, with multiple definitions arising in the vast recent literature
fitting different application scenarios and leading to different computational
approaches. A basic element shared by most frameworks is the definition and
evaluation of the probability of collision for a mobile object in an
environment with obstacles. We observe that, even in basic cases, different
interpretations are possible.

This paper proposes an index we call “Risk Density”, which offers a
theoretical link between conceptually distant assumptions about the interplay
of single collision events along a continuous path.

We show how this index can be used to approximate the collision probability in
the case where the robot evolves along a nominal continuous curve from random
initial conditions. Indeed under this hypothesis the proposed approximation
outperforms some well-established methods either in accuracy or computational
cost.

###### Index Terms:

risk, collision checking, robotics, safety, probability

##  I INTRODUCTION

As more and more autonomous systems are asked to perform safety-critical
tasks, the importance of evaluating the _risk_ associated with a set of
actions of a robot is becoming paramount. Several applications, including
warehouse management, autonomous driving, and cooperative robotics, require
the system not only to be _safe_ at any instant in time, but to plan actions
that assure safety also in the future [1]. Uncertainty plays a central role in
the process of associating a measure of how problematic planned actions could
be. Indeed, what the agent knows about itself and its environment drastically
affects the potential risk of its plan.

More in general, the debate on what constitutes a _risk_ spans various
research fields, ranging from economics [2, 3] to the studies of dynamical
systems [4]. Generally speaking, a measure of risk associates to the
_probability_ of a hazard weighted by its _severity_. The instruments of
stochastic analysis and optimization are often used to gauge and minimize
risk, since one or more of the variables involved are random. Even when the
control actions are deterministic, the outcome of these actions could still
follow a possibly unknown probability density function. To account for such
stochasticity in an optimization setting, researchers developed the tools of
risk-aware measures [4, 5, 6], which are mathematical operators that account
for higher order moments of the probability distribution of the cost. Chance
constraints [7, 8, 9, 10] indicate techniques used to ensure that imposed
restrictions on the states hold in a probabilistic sense.

One definition of risk commonly adopted by robotic literature is “the
probability of the robot not being able to finish the path” [11]. If we
consider collisions to be the sole hazard, the previous definition can be
further specialized as “the probability of collision from start to finish”
[12], or “the probability of collision with an uncertain obstacle at any time-
step” in the discrete case [7].

Figure 1: Depiction of a Robot with uncertain position following a path in a
partially unknown environment. Even the definition of risk and collision
probability in complex scenarios is not obvious. This paper illustrates some
of the difficulties, and proposes a risk density index which can help in
solving them.

Even in this restricted context, the risk assessment problem is still not
considered solved, as all the previously discussed methods fail to capture all
aspects of collision risk. Specifically, in the case of probability
computation, approximation schemes fail to assess the true value of the
probability or are computationally prohibitive and usually depend on
parameters foreign to the problem setting. Moreover, even the very definition
of the probability of a continuous set of events (in our case, the collision
probability along a continuous path) is not univocally defined without further
assumptions about the process and the interaction between events happening at
different times.

The literature reports multiple methods to evaluate event probabilities, each
with merits and limitations. Montecarlo simulations [13, 14, 15] can
accurately estimate collision probability, but are computationally intensive
since estimating the desired distribution requires a large number of samples.
Approaches to lower the computational hardness of this method are possible:
e.g., [16] uses importance sampling techniques to make the simulations satisfy
real-time requirements.

Joint chance constraints (CC) [17] are another tool that, at its core,
requires an evaluation of the probability of a set of events or, in our case,
of the probability of collision along a path. CC apply naturally to the
problem since they are conceived to consider the joint probability of all
events (in our case, collisions) from the start to the finish of a trajectory.
Unfortunately, joint chance constraints generally have to be approximated to
be used, as their interdependent nature makes them hard to evaluate either
computationally or analytically.

CC evaluation can be computed either at once, or sequentially. In both cases,
several techniques have been developed to tackle this evaluation. In the first
category, we find Ellipsoidal approximations [17] that simplify the
multivariate integral of the joint constraint to be one-dimensional, offering
good computational performance, but approximating the actual value in a very
conservative way. Instead of evaluating the joint constraint at once,
sequential methods consist in subdividing the joint CC into individual CCs and
combining their appraisal. Indeed, in this case, the approximation to be
employed is two-fold: both the value of each singular CC and their mutual
interaction must be simplified for computation. In practice, either Boole’s
Lemma is used to relax the joint constraint into a sum of multiple simpler
scalar chance constraints [18], or the constraints are considered independent
variables and combined in a multiplicative way. Individual chance constraints
can be translated to deterministic ones using the approximations used for
joint constraints or by using more ad-hoc approaches [8, 19]. The additive
relaxation result of Boole’s Lemma is known to be conservative. However,
techniques such as risk-allocation [12, 20] can mitigate this by assigning a
larger “probability mass” to stages of the path where the collision is more
likely.

Occupancy grids methods [11, 21, 22] are another prevalent choice used for
obstacle avoidance. Such approaches discretize the robot workspace in cells
and assign an occupancy probability value to each cell to perform risk
evaluation. Grid-based methods [22, 21, 11], by considering each cell of the
grid independent from one another, use a multiplicative relaxation of the
cumulative probability. While being the most popular choice to compute the
collision probability along a path, grid-based approaches are limited. Indeed,
choosing a discretization mesh influences the computed value of the collision
probability [23].

All the previous approaches, except for Montecarlo sampling, work well for
linear problems and Gaussian random variables. More general approaches exist,
able to tackle both nonlinear systems and non-Gaussian random variables via
Lasserre Hierarchy [24], [7], but these are computationally prohibitive.

###  I-A Contribution

Our contribution is to illustrate some of the problems encountered in applying
Grid-Based and Joint Chance Constraints methods and to propose a novel method
to characterize and approximate the robot collision probability along a
continuous trajectory. To do so, we focus on a well-defined case with
deterministic dynamics, where a rigorous definition of the collision
probability along a continuous path can be given. The proposed approximation
shows better or comparable accuracy to other approaches while being
computationally competitive. This approximation depends on the objects’
combined dimension and a quantity, which we call risk density, that depends
only on the parametrization of the path followed by the robot, the obstacle
position, and no exogenous parameters. Additionally, this metric gauges the
continuous path as a whole instead of subdividing it into waypoints.

The paper is organized as follows. Section II explores how the probability of
intersection between two uncertain shapes can be formalized and computed. In
Section III we discuss how this formalization translates to the problem of
collision probability computation along a continuous path. In this section, we
discuss some possible assumptions about the interdependence of events and
define the probability of collision as a consequence; secondly, we discuss an
example of how this probability can be computed and highlight some issues due
to the continuity of the problem. In Section IV, we propose the _Risk Density_
index, briefly discuss its relevance and introduce an approximation to the
collision probability along a continuous path. Section V briefly describes
current approaches and their limitations. In Section VI, we display the
computational experiments we have carried out as validation and compare our
approximation performance to other techniques. Finally, in Section VII, we
discuss the properties of the approach and how these are revealed in the
benchmark proposed while also highlighting the limitations of the method.
Section IX concludes the paper.

##  II BACKGROUND

Consider a robot and an obstacle immersed in a workspace
$\mathbb{W}\subseteq\mathbb{R}^{n}$. Let
$\mathbb{X}_{R}(x_{R})\subseteq\mathbb{W}$ be the set of points occupied by
the robot and $\mathbb{X}_{O}(x_{O})\subseteq\mathbb{W}$ the set occupied by
the obstacle, where $x_{R}\in\mathbb{W}$ and $x_{O}\in\mathbb{W}$ are the
reference frames locations for the robot and obstacle, respectively.

The overlap condition [25, 8] defined as

$C(x_{R},x_{O}):\mathbb{X}_{R}(x_{R})\cap\mathbb{X}_{O}(x_{O})\neq\\{\emptyset\\}\,,$
(1)

indicates wherever the two sets of points intersect, or in other words, that a
collision has happened if we consider a dynamic scenario.

The GJK algorithm [26] is a standard practical tool to check eq. 1 when all
the information about the shape and position of both the robot and object is
wholly known. One of its building blocks is the notion of Minkowski sum on
sets,

$A\oplus B:=\\{a+b|a\in A,b\in B\\},$ (2)

which can be used to define an equivalent condition to eq. 1, which is

$C(x_{R},x_{O}):\\{0\\}\subseteq\mathbb{X}_{R}(x_{R})\ominus\mathbb{X}_{O}(x_{O}),$
(3)

Where $A\ominus B\triangleq A\oplus(-B)$ is sometimes known as Minkowski
difference.

Unfortunately, the GJK algorithm is rendered useless as soon as uncertainty is
introduced in the problem. In fact, even assuming that both the obstacle and
the robot are perfectly known in shape but uncertain in position, condition
eq. 1 cannot be checked in a deterministic way anymore, and we are required to
define and appraise an overlap/collision probability.

A possible way to define such a probability is to resort to an _indicator
function_ , defined from (1) as

$I_{C}(x_{R},x_{O})=\begin{cases}1&\text{if
}\mathbb{X}_{R}(x_{R})\cap\mathbb{X}_{O}(x_{O})\neq\\{\emptyset\\},\\\
0&\text{otherwise.}\end{cases}$ (4)

Now, if we consider $x_{R}$ and $x_{O}$ to be realizations of two random
variables, $R$ and $O$, characterized by a joint probability density function
$f_{R,O}(r,o)$, the overlap or collision probability is

$P(C)=\int_{\mathbb{W}}\int_{\mathbb{W}}I_{C}(r,o)f_{R,O}(r,o)drdo\,.$ (5)

While (5) gives the actual value of $P(C)$, its numerical computation is, in
general, not trivial as it consists of two multidimensional integrals in
$\mathbb{R}^{n}$, where at each point $(r,o)$ one should calculate $I_{C}$,
e.g. via GJK. However, that analysis can be simplified by resorting to (3) and
shifting to another perspective. In a similar fashion to [19], we decompose
the uncertain sets $\mathbb{X}_{R}$ and $\mathbb{X}_{O}$ as

$\mathbb{X}_{R}=S_{R}+d_{R}+N_{R}(\omega),$ (6)
$\mathbb{X}_{O}=S_{O}+d_{O}+N_{O}(\omega).$ (7)

Now $S_{R}\subseteq\mathbb{W}$ and $S_{O}\subseteq\mathbb{W}$ are sets
centered in the origin of the deterministic shapes, $d_{R},\
d_{O}\in\mathbb{W}$ are vectors equal to the mean of $R$ and $O$ respectively,
while $N_{R}$ and $N_{O}$ are zero-mean Random Variables such that
$R=d_{R}+N_{R}$, $O=d_{O}+N_{O}$.

Condition (3) becomes equivalent to

$\displaystyle\\{0\\}\subseteq\mathbb{X}_{R}(R)\ominus\mathbb{X}_{O}(O)$
$\displaystyle\iff$ $\displaystyle\\{0\\}\subseteq(S_{R}\ominus
S_{O})+(d_{R}-d_{O})+(N_{R}-N_{O})$ $\displaystyle\iff$
$\displaystyle\\{0\\}\subseteq S_{RO}+d_{RO}+N_{RO}$ $\displaystyle\iff$
$\displaystyle N_{OR}\in S_{RO}+d_{RO}$ (8) $\displaystyle\iff$ $\displaystyle
d_{OR}+N_{OR}\in S_{RO},$ (9)

where

$d_{RO}=d_{R}-d_{O}=-d_{OR},$ (10)

is the distance vector between the mean of the two random variables and

$N_{RO}=N_{R}-N_{O}=-N_{OR},$ (11)

is a zero-mean Random Variable describing the uncertainty of such a vector.

Figure 2: Graphical depiction of the decomposition of the robot and obstacle
sets. The result of the Minkowski sum is also illustrated. Each object is
represented by its shape, the vector pointing to its frame of reference, and a
Random Variable describing the uncertainty in its position.

Using conditions (8) and (9), both derived from the set decomposition, (5) can
be restated in as either

$\displaystyle P(C)$ $\displaystyle=$
$\displaystyle\int_{S_{RO}}(f_{N_{OR}}(x)*\delta_{d_{OR}}(x))dx$
$\displaystyle=$ $\displaystyle\int_{S_{RO}}f_{N_{OR}}(x+d_{OR})dx$ (12) or
$\displaystyle P(C)$ $\displaystyle=$
$\displaystyle\int_{S_{RO}+d_{RO}}f_{N_{OR}}(x)dx\,.$ (13)

Here $f_{N_{OR}}=f_{N_{O}}*f_{N_{-R}}$ is the probability density function
resulting from the difference $N_{O}-N_{R}$, while $\delta_{d_{OR}}(x)$ is the
Dirac delta function centered in $d_{OR}$. Both eq. 12 and eq. 13 are computed
via a single integral on a defined set in contrast to eq. 5. All the remaining
complexity hides inside $f_{N_{OR}}$ since, in general, computing this pdf
requires a convolution operation.

This may not be an issue in the case when the convolution has closed form
expression, e.g. if $R$ and $O$ are normally distributed, i.e.
$R\sim\mathcal{N}(\mu_{R},\Sigma_{R})$,
$O\sim\mathcal{N}(\mu_{O},\Sigma_{O})$. In that case eq. 12 and eq. 13
simplify [27] to

$\displaystyle P(C)$ $\displaystyle=$
$\displaystyle\int_{S_{RO}}\mathcal{N}(x|\mu_{O}-\mu_{R},\Sigma_{T})dx$ (14)
$\displaystyle P(C)$ $\displaystyle=$
$\displaystyle\int_{S_{RO}+(\mu_{R}-\mu_{O})}\mathcal{N}(x|0,\Sigma_{T})dx,$
(15)

where $\Sigma_{T}=\Sigma_{R}+\Sigma_{O}$.

##  III PROBLEM STATEMENT

Having formalized the probability of a collision event in a fixed
configuration in Section II, we now approach the problem of defining the same
quantity along a path followed by the robot. A path is a function
$\mu_{R}(s):\mathbb{S}\to\mathbb{W}$ going from a possibly continuous ordered
set of configurations $\mathbb{S}=\\{s_{0},s_{1},\dots,s_{N}\\}$ to the
workspace. The integration set associated with each configuration $s$ is

$D_{RO}(s)=S_{RO}+(\mu_{R}(s)-\mu_{O}).$ (16)

Parametrizing the condition eq. 8, we associate to each configuration $s$ the
collision event

$C(s)=C_{s}:=\\{N_{RO}(\omega,s)\in D_{RO}(s)\\},$ (17)

meaning that each configuration has an associated probability in the form of
eq. 15 as

$P(C_{s})=\int_{D_{RO}(s)}\mathcal{N}(x|0,\Sigma_{T})dx.$ (18)

In general both the robot and the obstacle dynamic can be stochastic, making
$N_{RO}(\omega,s):\Omega\times\mathbb{S}\to\mathbb{W}$ a random process where
each random variable is correlated to the history of preceding configurations.
In general to model the dynamics of such a process, the tools of Itô Calculus
[28] are to be used. However in this paper we assume that the stochastic
process is identically distributed, i.e.
$N_{RO}(\omega,s_{i})\sim\mathcal{N}(0,\Sigma_{T})$.

While eq. 18 is valid for an isolated configuration $s$, interactions between
neighboring configurations in $\mathbb{S}$ must be accounted for if a path is
considered.

Indeed what we are interested in is the value of

$P(C):=P\left(\bigvee_{\mathbb{S}}C_{s}\right),$ (19)

which however is not directly computable due to the non trivial dependence
between events (17).

###  III-A Assumptions

To model such interactions, and to compute eq. 19, it is necessary to specify
further hypotheses which apply to different cases.

An assumption that is commonly adopted is to consider that all collision
events are independent (e.g. [29]). Formally, this model can be described as

  1. (H1)

$C(s_{i})\ \bot\ C(s_{j}),\ \forall s_{i}\neq s_{j}\in\mathbb{S},$

leading to the following lemma.

######  Lemma 1 (H1).

The probability of collision eq. 19 under (H1) reduces to

$P_{H1}(C)=1-\prod_{\mathbb{S}}(1-P(C_{s})),,$ (20)

where $P(C_{s})$ is eq. 18.

###### Proof.

$\displaystyle
P\left(\bigvee_{\mathbb{S}}C_{s}\right)=P\left(\overline{\bigwedge_{\mathbb{S}}\overline{C}_{s}}\right)=1-P\left(\bigwedge_{\mathbb{S}}\overline{C}_{s}\right)\xrightarrow[\text{ind}]{}$
$\displaystyle\xrightarrow[\text{ind}]{}1-\prod_{\mathbb{S}}(1-P(C_{s})),$
(21)

∎

While this premise is reasonable in the discrete case, it is easy to show that
it leads to incongruities as we approach a continuous path. As pointed out by
[16], the assumption is “asymptotically tautological”, as the estimation of
the collision probability resulting from (H1) converges to $1$ with the
cardinality of the set $|\mathbb{S}|\to\infty$. Hence with increasing the path
discretization resolution, all paths would almost surely lead to collision.

To fix this issue (H1) needs to be relaxed. A less restrictive alternative to
the independence hypothesis is the assumption that the collision event is
Markovian: the events $C_{s}$ depend only on the immediately preceding past,
i.e.
$P(C_{s_{j}}|\overline{C}_{s_{j-1}},\dots,\overline{C}_{s_{0}})=P(C_{s_{j-1}}|\overline{C}_{s_{j}})$.
In this case the conditioned probability is analytically unapproachable in
general, and approximations about its formulations are to be introduced. For
instance, the authors of [30] use truncated Normal distributions to model
preceding configurations being collision-free. We introduce here a different
simplification: we consider that the collision could only happen on newly
swept “area” in $\mathbb{W}$ at each index $s_{i}$. More precisely we assume
that

  1. (H2)

$C_{s}$ has the Markov property, and

  2. $P(C_{s_{i+1}}|\overline{C}_{s_{i}})=P(N_{RO}(s_{i+1})\in(D_{RO}(s_{i+1})\cap D_{RO}^{C}(s_{i})))$, where $D_{RO}^{C}(s)$ denotes the complement of the set $D_{RO}(s)$.

######  Lemma 2 (H2).

The probability of collision eq. 19 under (H2) reduces to

$P_{H2}(C)=1-\prod_{s_{i}\in\mathbb{S}}(1-P(C_{s_{i+1}\cap\overline{s}_{i}})),$
(22)

where
$C_{s_{i+1}\cap\overline{s}_{i}}:=\\{N_{RO}(s_{i+1})\in(D_{RO}(s_{i+1})\cap
D_{RO}^{C}(s_{i}))\\}$.

###### Proof.

$\displaystyle
P\left(\bigvee_{\mathbb{S}}C_{s}\right)=P\left(\overline{\bigwedge_{\mathbb{S}}\overline{C}_{s}}\right)=1-P\left(\bigwedge_{\mathbb{S}}\overline{C}_{s}\right)\xrightarrow[\text{mark}]{}$
$\displaystyle\xrightarrow[\text{mark}]{}1-\prod_{s_{i}\in\mathbb{S}}(1-P(C_{s_{i+1}}|\overline{C}_{s_{i}}))=$
$\displaystyle=1-\prod_{s_{i}\in\mathbb{S}}(1-P(C_{s_{i+1}\cap\overline{s}_{i}})).$

∎

Figure 3: Visual representation of how considered assumptions work on two
subsequent events. The events considered are $\\{N_{RO}(s_{0})\in A\\}$ and
$\\{N_{RO}(s_{0})\in B\\}$, where the sets $A$ and $B$ are shown as red and
blue disks. $P_{0}(\cdot)$ and $P_{1}(\cdot)$ stand respectively for the
probability measure of the first and the second random variable to be inside a
set.

An alternative model underpinning a different path integral extension of the
collision probability formula stems from considering the evolution of the
robot path as completely deterministic, while limiting the uncertainty to the
sole knowledge of the initial obstacle and robot position. This premise can be
formally stated using the concept of stopped process [31].

  1. (H3)

$N_{RO}(\omega,s)$ is a stopped process
$N^{s_{0}}_{RO}(\omega,s):=N_{RO}(\omega,\min(s_{0},s))$.

In this case the stochastic process is constant, as $s_{0}$ is the first
element of $\mathbb{S}$, meaning that and all the uncertainty can be
represented as a single Random Variable $N^{s_{0}}_{RO}(\omega)$ sampled from
a sample space $\Omega$.

We then introduce eq. 19 under (H3):

######  Lemma 3 (H3).

The probability of collision eq. 19 under (H3) reduces to

$P_{H3}(C)=\int_{D_{T}}\mathcal{N}(x|0,\Sigma_{T})dx,$ (23)

with

$D_{T}=\bigcup_{\mathbb{S}}D_{RO}(s).$ (24)

###### Proof.

$\bigvee_{\mathbb{S}}C_{s}=\bigvee_{\mathbb{S}}\\{N_{RO}(\omega,s)\in
D_{RO}(s)\\}=\bigvee_{\mathbb{S}}\\{N_{RO}(\omega,s_{0})\in
D_{RO}(s)\\}=\\{N_{RO}(\omega,s_{0})\in\bigcup_{\mathbb{S}}D_{RO}(s)\\}$, of
which the probability is eq. 23. ∎

Note that lemma 2 is a specialization of eq. 15 on the set $D_{T}$. Moreover,
notice how by swapping $D_{T}$ with the union of the $D_{RO}(s)$ over any
contiguous subset of $\mathbb{S}$, i.e.
$D_{RO}(s_{1},s_{2})=\bigcup_{s=s_{1}}^{s_{2}}D_{RO}(s)$, we obtain the
probability of the related subpath. In this sense the “Probability To Go” is
computed by integrating eq. 23 over $D_{RO}(s,s_{N})$.

While hypotheses such as (H1), (H2) and (H3) are mathematical abstractions,
these tightly connect to the assumptions one can draw from the particular
scenario. Indeed, (H1) can model fairly well an obstacle whose position
changes over time with a dynamics that is much faster than that of the robot,
but occupies, on average, the same regions of space with a fixed frequency:
think of obstacles such as a stream of people, or falling debris, or droplets
of water. (H2) instead models scenarios in which the collision can only happen
in newly explored areas, e.g. a robot vacuum in a house (without pets playing
around !). On the other hand (H3) would more accurately model a scenario
where, e.g., the obstacle is a pillar whose position is fixed but uncertain.

Notice that the complete set of possible scenarios form a somehow continuous
spectrum in which (H1) and (H3) lie at two opposite extremes: (H1) is total
independence between configurations, while (H3) denotes “total dependence”.
Multiple different modelling assumptions about the uncertainty can be found in
the range in between (H1) and (H3), such as (H2).

Figure 4: Interpretation of eq. 23. The problem is moved from the initial
domain $\mathbb{W}$ to the domain of the difference between the robot and the
obstacle shape. This results from the transformation shown in fig. 2, which
moves all the uncertainty on a single distribution centered in zero, while the
integration set is $D_{T}$.

###  III-B Continuous Path Parametrization

In the case in which the path $\mathbb{S}$ is continuous, one has either to
evaluate a union of the uncountable number of sets spanned by the robot moving
along the trajectory, eq. 24, or to compute the product over an uncountable
number of indices. Computing the former or the latter turns out to be
unpractical, or straight up impossible, although the path followed by the
robot can be parameterized as $\mu_{R}(s):[0,1]\to\mathbb{W}$, Therefore, a
parametrization of $D_{T}$ eq. 24 is needed. For simplicity let us assume that
$\mathbb{W}=\mathbb{R}^{2}$ and that $S_{R}$ and $S_{O}$ are disks. Then, the
combined shape $S_{RO}$ is a disk whose radius is the radii sum. Here $D_{T}$
takes the shape of a rounded tube, drawn by the large disk being swept along
the trajectory, as shown in fig. 4.

The characterization of such a swept space is generally rather complex.
Recently, that characterization has been addressed by [32] with nontrivial
mathematical methods, and focusing on local aspects.

Suppose a fully diffeomorphic map can parameterize the swept space

$\Phi(s,\theta):[0,1]\times[-T,T]\to\mathbb{W}\,,$ (25)

where $s\in[0,1]$ is a line variable defined along the path $\mu_{R}(s)$
through which the disk is dragged, and $\theta\in[-T,T]$ is a characteristic
length associated with the shape of the robot. Now, using eq. 25, eq. 23
becomes

$P^{a}_{H3}(C)=\int_{0}^{1}\int_{-T}^{T}\mathcal{N}(\Phi(\gamma,\nu)-\mu_{O}|0,\Sigma_{T})\Gamma(\gamma,\nu)d\nu
d\gamma\;,$ (26)

where $\Gamma(\gamma,\nu)=|\det(\nabla\Phi(\gamma,\nu))|$. As explored in
[33], eq. 25 can be used also to compute eq. 22 under (H2). Indeed, using the
parametrization, the conditional probability in the continuous case can be
rewritten as

$\displaystyle
P^{a}(C_{s+ds}|\overline{C}_{s})=\int_{s}^{s+ds}\int_{-T}^{T}\mathcal{N}(\Phi(\gamma,\nu)-\mu_{O}|0,\Sigma_{T})\Gamma(\gamma,\nu)d\nu
d\gamma$
$\displaystyle=\int_{-T}^{T}\mathcal{N}(\Phi(\gamma,\nu)-\mu_{O}|0,\Sigma_{T})\Gamma(\gamma,s)d\nu
ds.$ (27)

Plugging this in eq. 22 results in a Volterra integral [34], leading to

$P^{a}_{H2}(C)=1-\exp(-\int_{0}^{1}\int_{-T}^{T}\mathcal{N}(\Phi(\gamma,\nu)-\mu_{O}|0,\Sigma_{T})\Gamma(\gamma,\nu)d\nu
d\gamma)$ (28)

The problem with these approaches is that finding a parametrization
diffeomorphic to $\mathbb{W}$ is generally not trivial.

Consider indeed the naïve parametrization

$\Phi(s,\theta)=\begin{bmatrix}\mu_{R_{x}}(s)-\theta\frac{d\mu_{R_{y}}}{ds}(s)\\\
\mu_{R_{y}}(s)+\theta\frac{d\mu_{R_{x}}}{ds}(s)\end{bmatrix}.$ (29)

In this case the curve parametrization $\mu_{R}(s)$ is augmented by the
variable $\theta$, which indicates the “off-track” parameter, i.e. the locally
orthogonal deviation from $\mu_{R}(s)$. The boundary of the domain of $\theta$
is given by the parameter $T$, which for the space swept by an object of
circular shape amounts to the object radius $r$ (in our case, as the circle is
generated by two objects, $r=r_{r}+r_{O}$).

Note that eq. 29 is not a diffeomorphic map between the parametrization space
and $\mathbb{W}$, indeed it is only homeomorphic locally in a neighborhood
around $T=0$, unless the trajectory is a straight line. Therefore, whenever
$\mu_{R}(s)$ intersects itself or its curvature is not null, some region of
the integration set $D_{T}$ will be considered multiple times. Due to those
issues, the double integral eq. 26 does not always give a good approximation
of eq. 23. Besides that, evaluating eq. 26 can still be relatively expensive
to compute. These problems spawn from the continuity of the path as, even
under (H3), defining precisely the infinite set of unions $D_{T}$ for eq. 23
is, in general, analytically convoluted and computationally challenging.

We refer to the probability approximation computed by the integral eq. 26 with
parametrization eq. 29 as _Naïve Set Parametrization Approach_.

##  IV RISK DENSITY

Following the analysis of collision probability along a continuous path in
Section III, we investigate how the seemingly very different assumptions (H2)
and (H3) relate.

Indeed consider the sensitivity to the parameter $T$, denoting the bounds of
the variable $\theta$, of both eq. 26 and eq. 28 with parametrization eq. 29:

$\displaystyle
S_{r}^{H2}(\mu_{R},\overline{T}):=\frac{dP^{a}_{H2}}{dT}\Big{|}_{T=\overline{T}},$
(30) $\displaystyle
S_{r}^{H3}(\mu_{R},\overline{T}):=\frac{dP^{a}_{H3}}{dT}\Big{|}_{T=\overline{T}}.$
(31)

Notice the explicit dependence of eq. 30 and eq. 31 on the trajectory under
scrutiny $\mu_{R}(s)$ and the dimensional parameter $T$, as their knowledge
implies the parameterization eq. 29 .

######  Lemma 4.

The sensitivity under (H2) and under (H3) are equivalent in $T=0$, i.e.

$S_{r}^{H2}(\mu_{R},0)=S_{r}^{H3}(\mu_{R},0)$ (32)

###### Proof.

The risk sensitivity $S_{r}^{H2}(\mu_{R},\overline{T})$ and
$S_{r}^{H3}(\mu_{R},\overline{T})$ can be computed by the Liebniz Integral
rule as

$\displaystyle S_{r}^{H3}(\mu_{R},\overline{T})=$
$\displaystyle\int_{0}^{1}\left(\mathcal{N}(\gamma,\nu)\Gamma(\gamma,\nu)\right)\Big{|}_{\nu=\overline{T}}d\gamma+$
$\displaystyle\int_{0}^{1}\left(\mathcal{N}(\gamma,\nu)\Gamma(\gamma,\nu)\right)\Big{|}_{\nu=-\overline{T}}d\gamma,$
(33) $\displaystyle S_{r}^{H2}(\mu_{R},\overline{T})=$ $\displaystyle
S_{r}^{H3}(\mu_{R},\overline{T})\exp\left(-\int_{0}^{1}\int_{-\overline{T}}^{\overline{T}}\mathcal{N}(\gamma,\nu)\Gamma(\gamma,\nu)d\gamma
d\nu\right),$ (34)

where
$\mathcal{N}(\gamma,\nu):=\mathcal{N}(\Phi(\gamma,\nu)-\mu_{O}|0,\Sigma_{T})$.
In $T=0$, $\Gamma(\gamma,0)=\mu_{R}(s)$, and the preceding integrals both
evaluate to

$2\int_{0}^{1}\mathcal{N}(\mu_{R}(\gamma)-\mu_{O}|0,\Sigma_{T})\left|\frac{d\mu_{R}}{d\gamma}\right|d\gamma,$

where $|\cdot|$ is the 2-norm. ∎

The result is a functional taking a path parametrization $\mu_{R}$ and giving
a positive scalar as an output. We refer to this map as the _Risk Density_
along $\mu_{R}$, namely

$r_{d}(\mu_{R})=2\int_{0}^{1}\mathcal{N}(\mu_{R}(\gamma)-\mu_{O}|0,\Sigma_{T})\left|\frac{d\mu_{R}}{d\gamma}\right|d\gamma.$
(35)

Within the limits of the parametrization choice eq. 29, eq. 32 indicates that
(H2) and (H3) converge in the limit to the same formulation. The sensitivity
of the collision probability to small changes in the robot-obstacle dimensions
is the same in apparently very different conditions, i.e. the case when only
the initial position of the robot/obstacle is uncertain, or when the obstacle
can appear randomly in front of the robot.

Besides the interesting theoretical result that the concept of _Risk Density_
reconciles two very far points in the range of possible assumptions about
collision interaction, we argue that eq. 35 also has more practical uses. In
the rest of this paper we will show an approximation in the form

$P_{a}(C,T)\simeq rd(\mu_{R}(\cdot))T,$ (36)

can estimate collision probabilities.

##  V CURRENT COMPUTATIONAL APPROACHES

As already discussed in Section I, different approaches in literature dealt
with the estimation of collision probability along a continuous path. Here we
retrace how Grid-Based and Stage-Wise approximation methods deal with this
problem, highlighting some of their drawbacks.

###  V-A Grid-Based Methods

In their simplest form, Bayesian Grids [22] consist in a tessellation of the
environment and a set of Random Variables $\mathbf{m}$. Each element of
$\mathbf{m}$, $m_{i}$, is a binary Random Variable representing the occupancy
of the tile in the discretized space with associated probability
$P(m_{i})\triangleq P(m_{i}=1)$. With an implicit abuse of notation, we will
indicate with $m_{i}$ both the occupancy of the tile, and the tile itself.
Assuming the hypothesis, similar to (H1), that each cell forming the grid is
independent, enables the real-time capabilities of this approach, as the value
of each single cell can be updated without considering the joint probability
function of the entire map. Specifically, the quantity updated is the belief
(or posterior) probability of occupancy $P(m_{i}|z_{1:t},x_{1:t})$, given both
the set of the robot’s measurements $z_{1:t}=\\{z_{1},z_{2},\dots,z_{t}\\}$
and the set of its poses $y_{1:t}=\\{y_{1},y_{2},\dots,y_{t}\\}$ up to time
$t$. The value of the cells updates according to Bayes Rule111In practice, the
log-odds form of this relation is used, as it has better computational
performances.

$P(m_{i}|z_{1:t},y_{1:t})=\frac{P(z_{t},y_{t}|m_{i})P(m_{i}|z_{1:t-1},y_{1:t-1})}{P(z_{t},y_{t}|z_{1:t-1},y_{1:t-1})}.$
(37)

After each cell is updated at some fixed time $t$, the independence assumption
is used to compute the probability of collision along the path followed by the
robot.

Denote the set of the tiles crossed by the robot as $\mathbf{m}_{c}$ and the
collision event $m_{i}=1$ as $C_{i}$ (of which the belief is eq. 37).
Considering independent events, the probability of collision in the grid case
is

$P_{g}(C)=1-\prod_{m_{i}\in\mathbf{m_{c}}}(1-P(m_{i}|z_{1:t},y_{1:t})).$ (38)

As noted by [23] and others, one of the issues of this approach is that in no
way the area of the tiles is taken into consideration, as the belief
probability gets updated using only the sensor readings at time $t$.
Therefore, the discretization resolution influences the probability of
collision computed by the method, as shown in fig. 5.

| Red | Blue | Black
---|---|---|---
$|\mathbf{m_{c}}|$ | $3$ | $7$ | $19$
$P_{g}(C)$ | $1-\overline{P}^{2}_{a}\overline{P}_{b}$ | $1-\overline{P}^{4}_{a}\overline{P}^{3}_{b}$ | $1-\overline{P}^{10}_{a}\overline{P}^{9}_{b}$
| =0.757 | =0.982… | =0.999…

Figure 5: Example of discretization choice effect on the computed probability
by grid-based methods. The path crossed by the robot (green) lives in
$\mathbb{R}^{2}$, and $3$ distinct tessellations of the environment are given
(red, blue, black). The robot sensors measure different probability values eq.
37 for the cells depending on the image’s color. The probability computed
using eq. 38 assumes different values depending on the discretization,
although the underlying environment is the same. Indeed, the probability
changes as a function of the cardinality of the set $\mathbf{m_{c}}$.

Even if we circumvent this issue and compute $P(m_{i})$ instead of its belief
eq. 37, the independence assumption and the multiplicative formula eq. 38
would lead to incorrect conclusions, as we show numerically in Section VII.

###  V-B Stage-wise Probability Estimation Methods

(a) Joint chance constraint approximation.

(b) Probability approximations for Gaussian distributions.

Figure 6: Left: Example of under and over discretization of a continuous path
using Boole’s Lemma. Red circles represent the considered robot
configurations. The swept area $D_{T}$, in yellow, is misrepresented, whether
by selecting a set of configurations that is too sparse, leaving out much of
the event, or too dense, parametrizing intersections multiple times. Right:
Depiction of the approaches used by authors of [8] (top) and [35] (bottom) to
approximate Gaussian chance constraints. The boundary of the robot is in red.
The approximation integration set is shaded in purple, while the true
probability is in yellow.

Chance constraints are a popular framework used in optimization to address the
risk of collision. Specifically, chance constraints are inequalities involving
probabilities in the form of $P(C)\leq\delta$.

To convert individual chance constraints to scalar inequalities, either an
approximation or a bound on the probability of the event can be used. The
authors of [8] consider both the obstacle and the robot as normally
distributed objects of spherical shape and give an approximation of the
probability of collision eq. 15 as

$P(C)\approx
V_{S}\frac{1}{2\pi\sqrt{\det(\Sigma_{T})}}\exp{-\frac{1}{2}(\mu_{R}-\mu_{O})^{T}\Sigma_{T}^{-1}(\mu_{R}-\mu_{O})},$
(39)

where the variables used are the ones introduced in section II, and $V_{S}$ is
the area of the combined disk, in $\mathbb{R}^{2}$
$V_{S}=(r_{o}+r_{r})^{2}\pi$. The method amounts to taking the value of
$\mathcal{N}(x|0,\Sigma_{T})$ at $x=\mu_{R}-\mu_{O}$, the center of the
combined disk obtained as described in section II, and multiplying it for
$V_{s}$. While this approximation is efficiently evaluated and, as such, can
be implemented in dynamic planning methods [36], its value is not assured to
upper-bound the actual probability. An alternative to this is proposed in [37]
where an optimization problem is set up to compute $x_{max}$, the point inside
the combined disk that maximizes $\mathcal{N}(x|0,\Sigma_{T})$, and then
multiplying it for $V_{s}$: this always bounds the true probability from
above, as shown in fig. 6(b).

Whenever the event $C$ (i.e. the collision along a continuous path) to be
bounded in probability is constituted by the interaction of multiple events,
joint chance constraints are used. At the foundation of this framework lies
the idea of approximating the probability of collision eq. 19 along a
continuous path with the probability of the union of some discretized set of
configurations $\mathbb{I}\subset\mathbb{S}$ (waypoints) along the path,

$P(C)\approxeq P\left(\bigvee_{i\in\mathbb{I}}C_{i}\right).$ (40)

A bound on the right-hand side of eq. 40 is called Joint Chance Constraint.
Being Joint chance constraints computationally unapproachable, even in a
discrete scenario, relaxation techniques have been developed. Most of these
techniques rely on Boole’s Lemma eq. 41,

$P\left(\bigvee_{i\in\mathbb{I}}C_{i}\right)\leq\sum_{i\in\mathbb{I}}P(C_{i}),$
(41)

which over-bounds the probability of the union of the events with the sum of
their individual probabilities. We refer to this method as _Stage-Wise
Approximation_.

Unfortunately, eq. 40 does not tell how its two sides relate, meaning that the
bound on $P\left(\bigvee_{i\in\mathbb{I}}C_{i}\right)$ could be invalid for
$P(C)$. Indeed if the discretization chosen is too sparse, then the left-hand
side of eq. 41 will describe only a subset of the wanted event set, and the
probability computed could be a lower approximation of the true value $P(C)$,
as there is no accounting of the possibility of a collision happening between
waypoints. Conversely, by choosing an excessively dense discretization, the
gap between the bound and the true value of the probability grows, as shown in
fig. 6(a).

In practice, this approach tends to be very conservative. When used for
trajectory optimization, it rejects possibly feasible configurations [38].

##  VI COMPARISON OF COMPUTATIONAL APPROACHES

###  VI-A Setup

To analyze the proprieties of the proposed function, we introduce a simple
case study, compatible with the assumptions of Section II and (H3), which,
despite its simplicity, captures some of the problem richness.

A robot vehicle shaped as a disk of radius $r_{r}=0.05$, must navigate a room
with a flat floor with a round pillar of radius $r_{O}=0.05$, roughly located
in the middle of the room. Both the robot initial position and the pillar
location are known with some uncertainty that we can model as two independent
Gaussian distributions $N_{R}\sim\mathcal{N}(0,\Sigma_{R})$,
$N_{O}\sim\mathcal{N}(0,\Sigma_{O})$. We want to evaluate the collision
probability along three different paths $\mu_{A},\ \mu_{B},$ and $\mu_{C}$,
all starting from the origin $x_{0}$ and finishing $x_{F}=\begin{bmatrix}5\\\
0\end{bmatrix}$, while the nominal position of the obstacle is
$\mu_{O}=\begin{bmatrix}\frac{5}{2}\\\ 0\end{bmatrix}$. The first trajectory,
$\mu_{A}$, is a straight line crossing the nominal position of the obstacle,
$\mu_{B}$ and $\mu_{C}$ are parabolas of which the former is designed to graze
the obstacle in the nominal position, while the latter passes close to it
albeit always staying further than $r$.

Figure 7: Case in study: nominal case. Close-up of the set-up with the three
parabolic trajectories $\mu_{A},\mu_{B},\mu_{C}$. The trajectories are
parametrized as $\\{\begin{bmatrix}s\\\ 0\end{bmatrix},\begin{bmatrix}s\\\
-\frac{s^{2}-s}{2}\end{bmatrix},\begin{bmatrix}s\\\ s-s^{2}\end{bmatrix}\\}$.
The initial condition $x_{0}$ is in the origin, while
$x_{F}=\begin{bmatrix}5\\\ 0\end{bmatrix}$. The robot and the obstacle are
disks with radius $r_{r/O}=0.05$.

We look at how the probability value, computed with different approaches,
varies with regard to the magnitude of the covariance matrix $\Sigma_{T}$. To
look at the variation of this parameter we multiply a identity matrix by a
scalar value $\sigma$ so that $\Sigma_{T}=I*\sigma$; this parameter takes $10$
values spaced on a logarithmic scale in the range $(10^{-3},1)$.

####  VI-A1 Ground Truth

To compute what we take as the ground truth we perform a Montecarlo
simulation, consisting of $10000$ trials, for each of the $30$ cases in
scrutiny. At the start of each trial a random sample $\mu_{O}^{*}$ is taken
from $\mathcal{N}(x|\mu_{O},\Sigma_{T})$ and, given a discretization
resolution $ds$ for the curvilinear parameter $s$, at each step if
$|\mu_{i}(s)-\mu_{O}^{*}|\leq r_{r}+r_{o}$ then the simulation has stopped and
a collision is registered. From the Montecarlo’s results we compute the
probability ground-truth $P_{T}(C)$ for each of the inquired scenarios.

####  VI-A2 Naive Set Parametrization Probability Estimation

The approximation given by eq. 26, using the parametrization (29), is computed
for all the considered trajectories.

####  VI-A3 Grid-Based Probability Estimation

We evaluate the approximation given by the multiplicative formula eq. 38 in
various scenarios. The set of cells $\mathbf{m_{c}}:=\\{m_{1},\dots,m_{N}\\}$
which the robot intersects is obtained by rasterizing $D_{T}$ on the grid.

As it was observed, the belief value eq. 37 associated to each cell must be
corrected to take the size of the cell into account. We can do so by letting

$P(C_{m_{i}})=\int_{\mathcal{D}(m_{i})}\mathcal{N}(x|\mu_{O},\Sigma_{T})dx,$
(42)

where $\mathcal{D}(x)$ gives the dominion of the cell. This formula obviously
accounts for the area of the tiles. Then eq. 38 is rewritten as

$P_{g}(C)=1-\prod_{m_{i}\in\mathbf{m_{c}}}(1-P(C_{m_{i}}))$ (43)

In addition to the variation of the covariance matrix and the different
trajectories, in our benchmark we also consider different grid resolutions.

####  VI-A4 Stage-wise Probability Estimation

Combining the two approximations used by Chance Constraints, the _Stage-wise
approximation_ amounts to

$P_{cc}(C)\approx\sum_{i=1}^{N}P(C_{i})$ (44)

in which each $P(C_{i})$ is calculated as in eq. 40. Also, being $P_{cc}(C)$ a
probability, we consider the performance of the approximation by saturating it
to $1$. We also vary the number of waypoints $N$ linearly from $25$ to $300$.

####  VI-A5 Risk Density Based Probability Approximation

The approximation we employ is eq. 36 setting $T=r$, namely

$P_{r}(C)=rd(\mu_{R}(s))\cdot r,$ (45)

where $r=r_{r}+r_{o}$ is the combined radius. As it was the case for eq. 44,
we saturate the value of eq. 45 to $1$.

###  VI-B Comparison Metric

Denote $M_{T}\in[0,1]^{3\times 10}$ as the matrix containing the ground truth
for all the scenarios, and $M_{P}\in\mathbb{R}_{+}^{3\times 10}$ as the matrix
containing the same values computed by any of the presented approaches.

The error matrix is defined as

$M_{e}=M_{T}-M_{P}.$ (46)

The Frobenius Norm $||\cdot||_{F}$ and the maximum element in absolute value
$\max(|\cdot|)$ are used to compare the methods outlined in Section VI-A.

###  VI-C Results

Figure 8: Probability of Collision, Montecarlo Simulations. The heatmap shows
the probability of collision along the trajectories $\mu_{A}$, $\mu_{B}$, and
$\mu_{C}$, as a function of increasing values of the position error covariance
$\Sigma_{T}$. Each cell is the result of the ensemble of the Montecarlo Trials
for that combination of covariance matrix and trajectory. This heatmap is
considered the ground truth. Figure 9: Probability of Collision, Set
Parametrization. Values result of the integral eq. 26. We see how, while the
parametrization is accurate at evaluating the probability of collision of
$\mu_{A}$, the approximation starts to falter as soon as the curvature of the
trajectory is not null.  Figure 10: Probability of Collision, Grid based
method. The heatmaps show the value of the probability of collision with
regard of the size of the square tiling. The probability value associated to
each pairing of trajectory and $\sigma$ converges as the resolution of the
grid gets finer.  Figure 11: Probability of Collision, Grid based method -
tile size $2^{-9}$. As the resolution gets finer, the grid-based approach
converges to a value, which however is not the amount result of Montecarlo
simulation.  Figure 12: Probability of Collision, Stage-wise method. Result of
the approximation using eq. 44. The number of waypoints, in which the
approximation is computed, is varied. The values computed are ceiled at $1$.
We see how the approximation is not informative after a certain number of
waypoints as it always predicts a collision even in the safest of cases.

Montecarlo simulation results are displayed in fig. 8 for all scenarios. The
heatmap shows both the trend with respect to $\sigma$ and the trajectory
choice. While $\mu_{A}$ is clearly the riskier trajectory and presents a peak
in collision probability for very low values of $\sigma$, we see how the
probability of collision shows a maximum with respect to the covariance in
each path. At higher values of covariance, probability between the
trajectories tends to coincide.

The probability approximated by the set parametrization approach eq. 26 is
very accurate for the first trajectory, as shown in fig. 9, but quickly
degrades and overbounds as the trajectory gets further away and the curvature
increases.

The grid-based approximation eq. 43 estimations are shown in fig. 10. In each
of the heatmaps, the probability value is shown regarding uncertainty and the
path picked. In fig. 11 the Montecarlo simulation is shown along with the
values obtained from the grid-based approach with cell size $2^{-9}$. It is
apparent that the estimation converges to a specific curve by considering a
finer and finer grid, but this value approximates the ground truth poorly.

The heatmaps in fig. 12 present the behaviour of the stage-wise approximation
eq. 44. The method tends to be more and more conservative as the number of
waypoints $N$ increases. Moreover, saturation leads to a loss of information
about the trajectory, as the estimation predicts a collision regardless of the
trajectory. The value of the approximation can either be smaller or larger
than the actual probability depending on the number of waypoints, as shown in
fig. 13.

Figure 13: Probability approximation, Stage-wise approximation. Values result
of eq. 44, for some selected number of waypoints $N$, compared against
Montecarlo simulation. While usually this approach overbounds, notice how when
$N=25$ the probability of $\mu_{A}$ is lower than the actual value.  Figure
14: Risk Density. Behaviour of eq. 35 w.r.t. uncertainty scaling and path
chosen. While the vales shown are not probabilities, the behavior with respect
to $\sigma$ is maintained.  Figure 15: Probability of Collision, Risk Density.
Behaviour of the proposed approximation eq. 35 against Montecarlo simulation.

The trend of our _risk density_ eq. 35 considered in isolation is displayed in
fig. 14. The behaviour of the approximation eq. 45 is compared to the ground
truth given by Montecarlo in fig. 15.

A summarizing view is given in fig. 16. Here the errors against the ground
truth are plotted with regard to $\sigma$. The set parametrization approach
eq. 26, shown in light blue in fig. 16, is the most accurate in appraising the
risk of $\mu_{A}$, while the discrepancy with the Montecarlo ground truth is
larger the further the path is from the obstacle.

A more detailed comparison is given in table I. The table shows how our
approach gives a $12\%$ improvement in the Frobenius norm of the estimation
error matrix compared to the stage-wise approximation. The unsaturated values
are not included, as is evident how the stage-wise approximation greatly
overestimates the actual probability whenever the path in inquiry is “closer”
to the obstacle ($\mu_{A}$).

Figure 16: Errors against the Montecarlo simulation. The error is given in the $[-1,1]$ range: if the value plotted is positive, there is an overestimation of the probability, conversely, a negative value denotes an underestimation. The stage-wise approximation error shown is computed while fixing $N$, the number of waypoints, to $50$.  |  Stage-wise $(N=50)$ |  Parametrization eq. 29 |  Risk density $r$ |  $r_{opt}$($=0.1043$) |  Grid-based (Size$=2^{-9}$) |  Montecarlo ($N_{T}=10^{4}$)
---|---|---|---|---|---|---
$\max(|M_{e}|)$ | $0.2796$ | $0.3438$ | $0.2863$ | $0.2689$ | $0.7754$ | 0
$||M_{e}||_{F}$ | $0.649$ | $0.698$ | $0.579$ | $0.545$ | $2.869$ | 0
Comp Time $[s]$ | $(7.2\pm 2.2)\cdot 10^{-3}$ | $(8.7\pm 0.5)\cdot 10^{-1}$ | $(1.3\pm 0.2)\cdot 10^{-2}$ | $\sim$ | $380.43^{*}$ | $29\pm 3$
Table I: value of the matrix norm of the ensemble of errors $M_{P}$ and
computation time of considered methods. The computation time refers to the
execution of the whole set of $30$ scenarios. The stage-wise and risk density-
based approach errors are from the saturated approximation. The values in the
table were obtained by running each method $100$ times and computing the mean
time and its standard deviation. All the simulations were run on a modern
laptop with Matlab 2022b on an Intel i7-12700H with 32GB of Ram. ∗We include
the computation time for the grid-based computation for completeness, but our
simulation introduces complexity that would not be present in a standard
implementation.

##  VII COMPARISON’S DISCUSSION

As shown by the simulations, the accuracy of the approaches varies greatly,
even in the simple scenario considered above.

All reviewed methods capture the qualitative behavior shown by the Montecarlo
simulation, exhibiting a maximum in the probability estimate with respect to
the uncertainty scaling for all approaches. However, the parametrized
probability integral eq. 26 is not a reliable approximation even for modest
curvature values: this happens due to the fact that the parametrization eq. 29
of the integration set is not an homeomorphic map between $[0,1]\times[-T,T]$
and $\mathbb{W}$, leading to an overestimation of the actual probability
value. Moreover, the computation is $1-2$ orders of magnitude slower with
respect to other methods.

When the resolution of the cells gets finer, the grid-based method does not
converge to the probability predicted by the Montecarlo simulation. This fact
is mainly due to the difference between the assumption underlying the grid
approach and (H2): the latter assumes the independence of cells, which does
not apply to this scenario.

The stage-wise approximation eq. 44 of the probability of collision is fast
and, as such, suited to optimization. However, while eq. 39 is not guaranteed
to overbound the probability, we observe that for almost every number of
waypoints, the result of eq. 44 is a substantial overapproximation of the
ground truth. This conservativeness in an optimization setting could lead to
categorizing admissible states as unfeasible.

At last, the proposed function eq. 35, coupled with the approximation eq. 45,
shows some interesting properties. As no exogenous parameters are included in
its computation, we call it _path intrinsic_ ; namely, the probability eq. 45
is just a functional of the path $\mu_{R}(\cdot)$ taken by the robot and the
combined radius $r$, while no other tuning knob, such as $N$, the number of
waypoints, or the resolution of a grid, is considered222It could be argued
that the $r$ parameter by which the density eq. 35 is scaled in eq. 45 is not
the optimal one to estimate the probability of collision. To validate our
choice, we obtain $r_{opt}$ by minimizing $||M_{e}||_{F}$ for an arbitrary
choice of the scaling parameter. As shown in table I, the variation in the
values of the error is modest and does not justify the need for this
optimization.. Curiously, the approximation eq. 45 is more accurate than the
integral eq. 26: taking the derivative of eq. 26 at “thickness” $0$ rids eq.
45 of the local homeomorphicity issue encountered in eq. 26. Furthermore, the
method also seems to be competitive in regard to the accuracy of the
approximation, being the least conservative among the considered approaches.

While limited in scope, the experiment also shows that the computational cost
of the Risk Density approach is of the same order of magnitude ($\sim\times
2$) as the fastest method (Stage-wise approximation), as shown in table I.
This relatively small computational complexity is explained by the fact that
our approximation eq. 45 includes only a uni-dimensional integral, which is
easily evaluated numerically.

##  VIII RISK DENSITY AND THE SENSITIVITY OF COLLISION PROBABILITY

As shown in section VI, risk density can be used to give an approximation of
the collision probability when the position of the robot and the obstacle are
uncertain. We now depict another situation in which eq. 35 is useful. Let us
assume that we have already computed the probability of collision very
accurately (e.g. through a long running MC simulation) for an environment we
know the dimensions of very well. We call this estimate of the collision
probability $P_{M}^{(i)}$ and its associated dimensional parameter $T_{i}$.
Assume now that the dimension of the robot and/or the obstacle, i.e. the
environment, changes, while the trajectory followed by the robot, $\mu_{R}$,
is unvaried. The previously computed estimate of the collision probability is
now invalid, but computing the collision probability $P_{M}^{(i+1)}$ would
imply a hefty computational cost. To ameliorate this issue, we can use the
risk density based approximation eq. 36.

A first order approximation of $P_{M}^{(i+1)}$ given by the sensitivity is

$P_{M}^{(i+1)}\approxeq P_{M}^{(i)}+S_{r}^{Hi}(\mu_{R},T_{i})\cdot d_{T_{i}},$
(47)

where $Hi$ stands for either $H2$ or $H3$. We can obtain a similar relation by
exploiting the collision probability approximation given by the risk density.
Explicitly, when both the probabilities $P_{M}^{(i+1)}$ and $P_{M}^{(i)}$ are
approximated by eq. 36, the following can be written

$P_{M}^{(i+1)}\approxeq P_{M}^{(i)}+r_{d}(\mu_{R})\cdot d_{T_{i}},$ (48)

where $d_{T_{i}}=(T_{i+1}-T_{i})$. While the formulation eq. 48 should in
principle give a better estimation, it also requires to compute an integral
for each evaluation, i.e. section IV for every CP at a different $T_{i}$,
while eq. 48 requires to compute just once a uni-variate integral, the risk
density eq. 35.

###  VIII-A Numerical Validation Setup

To explore how the proposed approximation eq. 48 performs, we compute its
value and the result of a MC simulation (considered as ground truth) in a
slightly modified version of the setup introduced in Section VI. We maintain
the choice of paths and the range of $\sigma$, albeit lowering the resolution,
while we impose the combined disk radius $r$ to vary logarithmicly in the
range $[10^{-2},1]$: accordingly the value of $d_{T_{i}}$ at each index will
be $r_{i+1}-r_{i}$.

###  VIII-B Discussion of Results

The graph fig. 17 shows a comparison of the result of eq. 48 against the
ground truth, while the tables table II table III display both the absolute
error and the relative absolute error between the two, i.e.
$e_{a}=|P_{M}^{(i+1)}-P_{M}^{(i)}+r_{d}(\mu_{R})\cdot d_{T_{i}}|$ and
$e_{r}=e_{a}/P_{M}^{(i+1)}$.

We see how the proposed approximation follows the ground truth generally well
with few exceptions, as both the mean errors $e_{a}$ and $e_{r}$, are
relatively low, making the approach attractive even in comparison to eq. 47:
Table II show lower error values for eq. 47 in all but few cases, but the
differences are not drastic and there is additional computational cost
required. The limitations of the approach show up in the case in which the
trajectory is far from the obstacle relative to the covariance scaling
$\sigma$, e.g. in the set of pairs
$\\{(\mu_{B},10^{-3}),(\mu_{C},10^{-3}),(\mu_{C},10^{-2})\\}$, but while the
relative error $e_{r}$ is high, the absolute one is limited. The reason lies
in the fact that the risk density eq. 35 in these cases evaluate very close to
$0$ and eq. 48 output is very close to $P_{M}^{(i)}$, shown as an apparent
shift of the graphs in fig. 17.

Figure 17: Comparison of MC ground truth against approximations. The graphs show the CP estimate against the radius’ dimension. The MC ground truth is showed in Red, while the approximations are depicted in Blue eq. 48 and Yellow eq. 47. The grid is disposed to make the scrutinized pairing of path and $\sigma$ explicit.  $\bm{\mu}\ \backslash\ \bm{\sigma}$ | $\bm{10^{-3}}$ | $\bm{10^{-2}}$ | $\bm{10^{-1}}$ | $\bm{1}$
---|---|---|---|---
$\bm{\mu_{A}}$ | 2.33 1.25 [%] | 2.63 1.27 [%] | 2.23 1.26 [%] | 1.23 0.57 [%]
$\bm{\mu_{B}}$ | 9.96 3.15 [%] | 0.54 0.75 [%] | 2.21 1.43 [%] | 1.33 0.72 [%]
$\bm{\mu_{C}}$ | 10.00 1.36 [%] | 7.88 1.56 [%] | 2.22 1.39 [%] | 1.06 0.49 [%]
Table II: Mean absolute error w.r.t. the radius for eq. 48 (Blue) and eq. 47 (Yellow).  $\bm{\mu}\ \backslash\ \bm{\sigma}$ | $\bm{10^{-3}}$ | $\bm{10^{-2}}$ | $\bm{10^{-1}}$ | $\bm{1}$
---|---|---|---|---
$\bm{\mu_{A}}$ | 3.09 1.56 [%] | 8.81 1.95 [%] | 4.15 2.64 [%] | 4.78 3.69 [%]
$\bm{\mu_{B}}$ | 45.97 32.51 [%] | 2.26 2.37 [%] | 4.36 3.08 [%] | 4.23 3.26 [%]
$\bm{\mu_{C}}$ | 18.63 9.95 [%] | 22.56 8.78 [%] | 4.68 3.56 [%] | 5.01 4.14 [%]
Table III: Mean relative absolute error w.r.t. the radius for eq. 48 (Blue)
and eq. 47 (Yellow).

##  IX CONCLUSIONS

This paper presents an analysis of the sensitivity of collision probability to
small perturbations in the robot/object dimensions, and introduces the idea of
”risk density”. We show that this concept carries over different assumptions
about the interplay of collision events, modeling different practical
situations and offering a theoretical link between the hypotheses.

The derivation of such approximation starts from analyzing the intersection
probability of two uncertain shapes and then generalizing it to a continuous
path. After discussing this theoretical property of risk density, we showed
how a probability approximation scheme based on it can be applied to the case
of a robot moving in a deterministic unknown environment with uncertain
initial conditions. We have thus shown how the proposed approximation offers
accuracy and performance comparable to or better than other popular
probability estimation methods. We have also displayed how the risk density
can be used to avoid computing a Montecarlo simulation when a slight change in
environment occurs. The proposed risk-density based approximation does not
introduce the need for tuning parameters that are not intrinsic to the
problem.

Under appropriate assumptions, we expect the the risk density framework can be
used to handle realistic problems with multiple obstacles. For example,
assuming that collision with different individual obstacles are independent,
one can easily combine their probabilities by multiplication. This
straightforward extension could also be used to compute the collision
probability for large obstacles, e.g. by representing these as a collection of
small disks. These two steps will enable the implementation of the suggested
approximation in realistic trajectory planning problems. We defer the analysis
of such application to our future work.

### Limitations and Future Research

Our numerical comparisons were based on assumption (H3) only, i.e. uncertainty
only in the initial condition. While valid, the assumption is a simplification
of the true stochastic nature of the dynamics. Describing the collision event
as a stopped process eludes considering the conditioning between states: a
better approach would consider that. Our approach to computing collision
probability does not currently consider non-Gaussian and non-stationary
distributions for the uncertainty. Therefore, further investigation in this
regard is warranted. The fact that the _Risk Density_ definition coincides
between (H2) and (H3) encourages to explore its application to the (H2) case
as well. In the context of _Risk Density_ , exploring further the theoretical
link between (H2) and (H3) and how it can be translated to practical
approximations also under other hypotheses is of interest. Moreover, while in
this manuscript we have carried out only the analysis of the proprieties of
this novel index in $\mathbb{R}^{2}$, a generalized approach could be looked
for in higher dimensions. Finally, investigating the performance and
proprieties of the proposed approach in a path optimization scenario is also a
research avenue.

## References

  * [1] T. Fraichard, “A Short Paper about Motion Safety,” in _Proceedings 2007 IEEE International Conference on Robotics and Automation_. Rome, Italy: IEEE, Apr. 2007, pp. 1140–1145.
  * [2] G. A. Holton, “Defining Risk,” _Financial Analysts Journal_ , vol. 60, no. 6, pp. 19–25, Nov. 2004.
  * [3] P. Whittle, “RISK SENSITIVITY, A STRANGELY PERVASIVE CONCEPT,” _Macroecon. Dynam._ , vol. 6, no. 1, pp. 5–18, Feb. 2002.
  * [4] A. Majumdar and M. Pavone, “How Should a Robot Assess Risk? Towards an Axiomatic Theory of Risk in Robotics,” in _Robotics Research_ , N. M. Amato, G. Hager, S. Thomas, and M. Torres-Torriti, Eds. Cham: Springer International Publishing, 2020, vol. 10, pp. 75–84.
  * [5] A. Ahmadi-Javid, “Entropic Value-at-Risk: A New Coherent Risk Measure,” _J Optim Theory Appl_ , vol. 155, no. 3, pp. 1105–1123, Dec. 2012.
  * [6] F. Farshidian and J. Buchli, “Risk Sensitive, Nonlinear Optimal Control: Iterative Linear Exponential-Quadratic Optimal Control with Gaussian Noise,” _arXiv:1512.07173 [cs]_ , Dec. 2015.
  * [7] W. Han, A. Jasour, and B. Williams, “Non-Gaussian Risk Bounded Trajectory Optimization for Stochastic Nonlinear Systems in Uncertain Environments,” Mar. 2022.
  * [8] N. E. Du Toit and J. W. Burdick, “Probabilistic Collision Checking With Chance Constraints,” _IEEE Trans. Robot._ , vol. 27, no. 4, pp. 809–815, Aug. 2011.
  * [9] L. Blackmore and M. Ono, “Convex Chance Constrained Predictive Control Without Sampling,” in _AIAA Guidance, Navigation, and Control Conference_. Chicago, Illinois: American Institute of Aeronautics and Astronautics, Aug. 2009.
  * [10] A. Shapiro, D. Dentcheva, and A. Ruszczyński, _Lectures on Stochastic Programming: Modeling and Theory_. Society for Industrial and Applied Mathematics, Jan. 2009.
  * [11] X. Xiao, J. Dufek, and R. R. Murphy, “Robot Risk-Awareness by Formal Risk Reasoning and Planning,” _IEEE Robot. Autom. Lett._ , vol. 5, no. 2, pp. 2856–2863, Apr. 2020.
  * [12] Masahiro Ono and B. C. Williams, “Iterative Risk Allocation: A new approach to robust Model Predictive Control with a joint chance constraint,” in _2008 47th IEEE Conference on Decision and Control_. Cancun, Mexico: IEEE, 2008, pp. 3427–3432.
  * [13] L. Blackmore, M. Ono, A. Bektassov, and B. C. Williams, “A Probabilistic Particle-Control Approximation of Chance-Constrained Stochastic Predictive Control,” _IEEE Trans. Robot._ , vol. 26, no. 3, pp. 502–517, June 2010.
  * [14] A. Lambert, D. Gruyer, and G. Saint Pierre, “A fast Monte Carlo algorithm for collision probability estimation,” in _2008 10th International Conference on Control, Automation, Robotics and Vision_. Hanoi, Vietnam: IEEE, Dec. 2008, pp. 406–411.
  * [15] A. B. Owen, _Monte Carlo Theory, Methods and Examples_ , 2013.
  * [16] L. Janson, E. Schmerling, and M. Pavone, “Monte Carlo Motion Planning for Robot Trajectory Optimization Under Uncertainty,” May 2015.
  * [17] D. H. van Hessem, “Stochastic inequality constrained closed-loop model predictive control: With application to chemical process operation,” 2004.
  * [18] L. Blackmore, Hui Li, and B. Williams, “A probabilistic approach to optimal robust path planning with obstacles,” in _2006 American Control Conference_. Minneapolis, MN, USA: IEEE, 2006, p. 7 pp.
  * [19] J. S. Park, C. Park, and D. Manocha, “Efficient Probabilistic Collision Detection for Non-Convex Shapes,” Oct. 2016.
  * [20] L. Blackmore, M. Ono, and B. C. Williams, “Chance-Constrained Optimal Path Planning With Obstacles,” _IEEE Trans. Robot._ , vol. 27, no. 6, pp. 1080–1094, Dec. 2011.
  * [21] A.-a. Agha-mohammadi, “SMAP: Simultaneous Mapping and Planning on Occupancy Grids,” Sept. 2016.
  * [22] A. Elfes, “Using occupancy grids for mobile robot perception and navigation,” _Computer_ , vol. 22, no. 6, pp. 46–57, June 1989.
  * [23] J. Laconte, C. Debain, R. Chapuis, F. Pomerleau, and R. Aufrere, “Lambda-Field: A Continuous Counterpart of the Bayesian Occupancy Grid for Risk Assessment,” in _2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. Macau, China: IEEE, Nov. 2019, pp. 167–172.
  * [24] J. Lasserre, “The Moment-SOS hierarchy,” Aug. 2018.
  * [25] S. M. LaValle, _Planning Algorithms_. Cambridge: Cambridge University Press, 2006.
  * [26] E. Gilbert, D. Johnson, and S. Keerthi, “A fast procedure for computing the distance between complex objects in three-dimensional space,” _IEEE J. Robot. Automat._ , vol. 4, no. 2, pp. 193–203, Apr. 1988.
  * [27] H. Pishro-Nik, _Probability, Statistics & Random Processes_, 2014.
  * [28] J. L. Speyer and W. H. Chung, _Stochastic Processes, Estimation, and Control_ , 1st ed., ser. Advances in Design and Control. Philadelphia: Society for Industrial and Applied Mathematics, 2008, no. 17.
  * [29] J. van den Berg, P. Abbeel, and K. Goldberg, “LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information,” _The International Journal of Robotics Research_ , vol. 30, no. 7, pp. 895–913, June 2011.
  * [30] S. Patil, J. van den Berg, and R. Alterovitz, “Estimating probability of collision for safe motion planning under Gaussian motion and sensing uncertainty,” in _2012 IEEE International Conference on Robotics and Automation_. St Paul, MN, USA: IEEE, May 2012, pp. 3238–3244.
  * [31] R. G. Gallager, _Stochastic Processes: Theory for Applications_. Cambridge, United Kingdom ; New York: Cambridge University Press, 2013.
  * [32] Y. de Mont-Marin, J. Ponce, and J.-P. Laumond, “A minimum swept-volume metric structure for configuration space,” Nov. 2022.
  * [33] L. Paiola, G. Grioli, and A. Bicchi, “Considerations on possible approaches to measure risk for obstacle avoidance,” in _Fourth Italian Conference on Robotics and Intelligent Machines (I-RIM)_. Rome, Italy: Zenodo, Oct. 2022.
  * [34] A. Slavík, _Product Integration, Its History and Applications_ , ser. History of Mathematics. Praha: Matfyzpress, 2007, no. 29.
  * [35] C. Park, J. S. Park, and D. Manocha, “Fast and Bounded Probabilistic Collision Detection for High-DOF Trajectory Planning in Dynamic Environments,” _IEEE Trans. Automat. Sci. Eng._ , vol. 15, no. 3, pp. 980–991, July 2018.
  * [36] N. E. Du Toit and J. W. Burdick, “Robot Motion Planning in Dynamic, Uncertain Environments,” _IEEE Trans. Robot._ , vol. 28, no. 1, pp. 101–115, Feb. 2012.
  * [37] C. Park, J. S. Park, and D. Manocha, “Fast and Bounded Probabilistic Collision Detection for High-DOF Robots in Dynamic Environments,” in _Algorithmic Foundations of Robotics XII_ , K. Goldberg, P. Abbeel, K. Bekris, and L. Miller, Eds. Cham: Springer International Publishing, 2020, vol. 13, pp. 592–607.
  * [38] K. Frey, T. Steiner, and J. How, “Collision Probabilities for Continuous-Time Systems Without Sampling,” in _Robotics: Science and Systems XVI_. Robotics: Science and Systems Foundation, July 2020.