Lambda-Field: A Continuous Counterpart of the Bayesian Occupancy Grid for Risk Assessment

I Introduction

Autonomous vehicles are nowadays more and more visible in our life. Most of them can be found in warehouses or on the road. As they evolve in a complex world, they need to assess the best choices to make regarding of the possible obstacles. These choices are guided by the notion of risk: the robot has not to harm others or itself. Fraichard2007 introduced this notion for known obstacles: the robot has not to collide with others to ensure its safety.

The most common way to store and deal with obstacles are the occupancy grids [Elfes1989]. The map is discretized into a finite number of cells, where each cell stores the probability of occupancy. While navigating, the main concern is to ensure the safety of the robot and its surrounding. The most commonly used metric is the probability of collision, as the impact between the robot and another physical object is the main hazard [Fraichard2007]. In occupancy grid, we would be tempted to assess the probability of collision as the joint probability of colliding each cell. This simplicity hide a huge drawback that appears when computing this probability for two discretizations of the same map. Figure 1 shows a robot wanting to cross an environment where the probability of occupancy is $0.1$ . We discretized the environment with two different cell sizes. For the first one, we need to compute the probability that at least one cell is occupied, i.e., $1 - {0.9}^{4} \approx 0.34$ . For the second one, we only need to compute this probability over two cells, leading to $1 - {0.9}^{2} = 0.19$ . We see that probabilities of collision for crossing the same part of the environment are completely different and dependent on the discretization size. The grids however store the same information using different discretizations, thus should give the same probability of collision. This problem can also be encountered while dealing with occupancy grids stored in quad-trees [Kraetzschmar2004]. Indeed, the robot could decide to cross a large high-probability cell instead of ten small low-probability cells.

We define the risk as a quantification of the danger encountered along a path. More precisely, this risk will be quantified as the force of collision the robot expects from a path. It is indeed more ‘risky’ to hit a car at high speed than at low-speed.We propose in this article a novel method to compute the risk over a path. Our key contributions are:

A novel type of map, called lambda-field, specially conceived to allow path integrals over it;
A mathematical formulation of the collision probability over a path; and
A definition of the risk encountered over a path, specified as the expected force of collision along a path.

Section II presents a survey of the different methods to store the occupancy of the environment and the attempts to assess risk in these maps. Section III describes the theory of the lambda-field and shows the inherent application to risk assessment. Finally, we test in simulation our theory in Section IV, with a robot-follower scenario.

Fig. 1: The robots (black boxes with its front represented as a filled triangle) want to cross an environment by following the dashed red line. The collision probability is uniform for the whole environment ( $0.1$ ). The discretization size greatly influences the probability of collision, with the bottom scenario yielding to a safer path even though the underlying environment is the same.

Ii Related Work

When dealing with complex environments, it becomes difficult to describe the map in a topological way [Saval-Calvo2017]

. Instead, the map is discretized into cells, which carry the probability of occupancy. This concept was first introduced by Elfes1989, enhanced later with Bayesian estimation by Coue2003.

Recently, OCallaghan2016 used Gaussian process to store the occupancy map without discretization. The main drawback of this method is the processing complexity which can be quite expensive. Senanayake2017a introduced the concept of Hilbert maps, mainly to overcome the problems encountered while using Gaussian process. However, these techniques do not allow any good formulation of the probability of collision along a path. We hence present in this article a new way to store the occupancy of an environment, in a fashion that allows the computation of collision risk.

The most commonly measure of risk is the time-to-collision (TTC) introduced by Lee1976. This approach however assumes that all the obstacles are known and non probabilistic. Laugier2011 demonstrated some of its limitations.

Another approach is to cluster the obstacles, as done by Althoff2010 and Fulgenzi2007. That way, the probability of collision is the union of the probability of collision to each obstacle. As it is very tempting, the clustering and target-tracking is still an open problem. Furthermore, the probability of collision to static objects is not addressed. The notion of risk is indeed limited to moving obstacles, that are often considered as known. Fraichard2007 presented in this fashion a model of risk and safety, but for dynamic environments and known obstacles.

To our best of our knowledge, the notion of risk along a path has not been well-defined, as well as a way to compute it. We hence propose a new framework to allow computation of a risk (i.e., danger) along a path.

Works about planning in uncertain maps are relatively limited: the map is often assumed to contain only free and occupied cells (as done in Gerkey2008 for example). Indeed, the developed metrics of risk along a path in Bayesian occupancy grid lack of physical meaning. It is hence difficult to know at which point the robot should stop, and a hard threshold is defined. Missiuro2006 adapted the framework proposed by Kavraki1996 to cope with uncertainty in topological maps. The work is however not easily transposable to Bayesian occupancy grids. Rummelhard2014 defined the risk as the probability to collide with a specific area, as well as the maximum value of collision over the collided cells. This approach was mainly developed to overcome the limitations of the Bayesian occupancy grid. Heiden2017 proposed a way to plan trajectories in Bayesian occupancy grid using product integrals. Nevertheless, the last two metrics lack of a physical meaning. Our method extends their works, adding a physical meaning to the risk value, as well as taking into account the size of the robot.

Iii Theoretical Framework

The key concept of the lambda-field is its capability to assess the probability of collision inside a subset of the environment in which the robot evolves. It relies on the mathematical theory of the Poisson point process. This process counts the number of events which have happened given a certain period or area, depending of the mathematical space. In our case, we want to count the number of the event ‘collision’ which could occur given a path (i.e., a subset of $R^{2}$ ).

For a positive real-valued field $λ (x), x \in R^{2}$ , the probability to encounter at least one collision in a path $P \subset R^{2}$ is

P(coll|P)=1−exp(−∫Pλ(x)\difx).

(1)

Nonetheless, it is both impossible to compute and store the field $λ (x)$ . We hence discretize our field into cells in a similar fashion to Bayesian occupancy grids. Under the assumption that the cells are small enough, the probability of collision can be approximated by

P (c o l l | P) \approx 1 - exp (- Λ (C)), Λ (C) = A \sum c_{i} \in C λ_{i}

(2)

for a path $P$ crossing the cells $C = {c_{0}, \dots, c_{N}}$ , where each cell $c_{i}$ has an area of $A$ and an associated lambda $λ_{i}$ , which is the intensity of the cell. The higher that lambda is, the most likely an obstacle is in this cell.

Using this representation, we hereby see that the probability of collision is not dependent on the size of the cells. It is indeed the same to compute the probability of collision for crossing two cells of area $A / 2$ or one cell of area $A$ for a constant $λ$ . Figure 2 gives an example of a path the robot might take, as well as the underlying cells (of area $0.04 m^{2}$ ) it crosses. The robots crosses $58$ cells with $λ_{i} = 0.1$ and one cell with $λ_{i} = 2$ : using Equation 2, the probability of collision is evaluated at $1 - exp (- 0.04 (58 \cdot 0.1 + 1 \cdot 2)) \approx 0.27$ .

Fig. 2: The robot (black boxes with its front represented as a filled triangle) wants to go to the position in red. In blue, the actual path the robot might take. Each cell has an area of $0.04 m^{2}$ . Using the Equation 2, the probability of collision in this path is $0.27$ .

Iii-a Computation of the field

As we established a new approach to represent the occupancy of an environment, we need to develop a way to compute dynamically the lambdas. We assume that the robot is equipped with a lidar sensor, which gives us a list of cells crossed by beams without collision, and another list of cells where the beams collided. Also, we chose to represent the uncertainty of the sensor in a fashion that differs from the standard forward models [Elfes1989], given three variables:

$E_{k}$ , the region of error of the lidar for the beam $b_{k}$ with its associated area $e_{k}$ . It represents the accuracy of the sensor, and can be of any shape: it means that the true position of the obstacle is within the region $E_{k}$ , centered on the obstacle measurement from the beam $b_{k}$ . We chose to set the shape and size of $E_{k}$ constant for every measurements. We thus have $e_{k} = e$ for all beams.
$p_{m}$ , the probability of rightfully read ‘miss’ for a cell (i.e., the cell is not in the region of error $E_{k}$ ). The quantity $1 - p_{m}$ hence gives the probability to read ‘miss’ for a cell that should be in the region of error $E_{k}$ .
$p_{h}$ , the probability of rightfully read ‘hit’ for a cell (i.e., the cell is in the region of error $E_{k}$ ). The quantity $1 - p_{h}$ hence gives the probability to read ‘hit’ for a cell that is empty. The probability $p_{h}$ is for example much lower when the sensor is in the rain, as many readings comes from the raindrops.

We can directly convert a Bayesian occupancy grid into a lambda-field. To do so, we need to give a meaning to the probability of occupancy contained in every cell, which is not dependent on the size of the cell. Otherwise the $λ$ deduced from the probability of occupancy would be dependent on the size of the cell, hence losing all purpose. A valid approach is to say that the probability $p$ means that we would have the probability $p$ of colliding if the cell had an area of $e$ . Indeed, it means that we are sure that every lidar reading truly hit this cell. Mathematically speaking, we would have for any cell $c$

with a Bayesian probability

p

the corresponding

λ

, defined as

	$P (c o l l \| c)$	$=1−exp(−∫Eλ\difx)=p$		(3)
	$\Leftrightarrow$	$λ = - \frac{ln (1 - p)}{e} .$		(3)

Nevertheless, it is far more efficient and logical to directly fill the lambdas from the lidar’s data. We developed an approximate closed-form from the Expectation-Maximization algorithm. We want to find the combination of

λ = {λ_{i}}

that maximizes the expectation of the

K

beams the lidar has shot since the beginning. For each lidar beam

b_{k}

, the beam crossed without collision the cells

m \in M_{k}

and hit an obstacle contained in the cells

h \in E_{k}

. The log-likelihood of the beam

b_{k}

L (b_{k} | λ) = ln [exp (- Λ (M_{k})) (1 - exp (- Λ (E_{k})))] .

(4)

The log-likelihood of $K$ lidar beams is then

		$L ({b_{k}}_{0 : K - 1} \| λ) = K - 1 \sum k = 0 L (b_{k} \| λ)$		(5)
		$= K - 1 \sum k = 0 [- Λ (M_{k}) + ln (1 - exp (- Λ (E_{k})))] .$		(5)

We want to maximize this quantity, hence nullify its derivative since the function is concave. The derivative of the log-likelihood equals to

\frac{\partial L ({b_{k}}_{0 : K - 1} | λ)}{\partial λ_{i}} = - m_{i} \cdot A + K - 1 \sum k = 0 \frac{A}{exp (\sum_{h \in E_{k}} λ_{h}) - 1},

(6)

where $m_{i}$ stands for the number of times the cell $c_{i}$ has been counted as ‘miss’.

We can approximate this equation with the assumption that the variation of lambda inside the region of error of the lidar is small enough to be negligible. Thus, for each $λ_{i}$ we have

\sum h \in E_{k} λ_{h} \approx e λ_{i} .

(7)

Using this approximation, the derivative becomes

\frac{\partial L ({b_{k}}_{0 : K - 1} | λ)}{\partial λ_{i}} \approx - m_{i} \cdot A + h_{i} \frac{A}{exp (e λ_{i}) - 1},

(8)

where $h_{i}$ is the number of times the cell $c_{i}$ has been counted as ‘hit’ (i.e., was in the region of error of the sensor).

Finally, we can find the zero of this derivative for each lambda, leading to

\Leftrightarrow λ_{i} = \frac{1}{e} ln (1 + \frac{h_{i}}{m_{i}}) .

(9)

This closed-form allows very a quick computation of the lambda field. We also see that the formula does not depend on the size of the cells, as expected.

Iii-B Confidence intervals

In the same way as Agha-mohammadi2016, we define the notion of confidence over the values in the lambda-field. Indeed, the robot should not be as confident over a certain path if the cells have been read one time or one hundred times. For each cell $c_{i}$ , we seek the bounds $λ_{L}$ and $λ_{U}$ such that

		$P (λ_{L} \leq λ_{i} \leq λ_{U}) \geq 95 %$		(10)
	$\Leftrightarrow$	$P (λ_{L} \leq \frac{1}{e} ln (1 + \frac{h_{i}}{m_{i}}) \leq λ_{U}) \geq 95 % .$		(10)

Using the relation $h_{i} = M - m_{i}$ where $M$ is the number of times the cell has been measured, we can rewrite the above equation as

P (K_{L} \leq h_{i} \leq K_{U}) \geq 95 %,

(11)

such that

	$λ_{L}$	$= \frac{1}{e} ln (\frac{K_{L}}{M - K_{L}} + 1)$		(12)
	$λ_{U}$	$= \frac{1}{e} ln (\frac{K_{U}}{M - K_{U}} + 1) .$		(12)

The quantity $h_{i}$ can be seen as a sum of two binomial variables:

	$h_{i}$	$= ¯ h + (m_{i} - ¯ m),$	$¯ h \sim B (p_{h}, h_{i})$		(13)
		$= ¯ h + ¯ w$	$¯ w \sim B (1 - p_{m}, m_{i}),$		(13)

where $¯ h$ (resp. $¯ m$ ) is the number of times the sensor rightfully read a ‘hit’ out of the $h_{i}$ trials (resp. read a ‘miss’ out of the $m_{i}$ trials). The quantity $(m_{i} - ¯ m)$ is hence the number of times the sensor wrongfully read ‘hit’ instead of ‘miss’.

The distribution of $h_{i}$

is not binomial but a Poisson binomial distribution with poor behaviors in terms of computation. Since the Poisson binomial distribution satisfies the Lyapunov central limit theorem, we can approximate its distribution with a Gaussian distribution of same mean and variance:

	$μ$	$= h_{i} p_{h} + m_{i} (1 - p_{m}),$		(14)
	$σ^{2}$	$= h_{i} (1 - p_{h}) p_{h} + m_{i} (1 - p_{m}) p_{m} .$		(14)

We can then have the bounds at 95%, with

	$K_{L}$	$= μ + σ \sqrt{2} {erf}^{- 1} (2 \cdot 0.025 - 1) \approx μ - 1.96 σ,$		(15)
	$K_{U}$	$= μ + σ \sqrt{2} {erf}^{- 1} (2 \cdot 0.975 - 1) \approx μ + 1.96 σ$		(15)

where $erf (\cdot)$ is the standard gaussian error function. $λ_{L}$ and $λ_{U}$ are then retrieved from $K_{L}$ and $K_{U}$ using Equation 12.

Figure 3

shows examples of behavior of the confidence intervals for different confidences. The lidar measures an empty cell

c_{i}

. The confidence interval quickly decrease as the number of readings ‘miss’ increases. At the fortieth measurement, the lidar misreads and returns a ‘hit’ for the cell. The confidence interval grows around the expected lambda computed with Equation 9 before reconverging.

Fig. 3: Convergence of the confidence intervals for a free cell $c_{i}$ . At the fortieth measurement, the sensor misread the cell and return a ‘hit’. The confidence interval grows around the expected $λ_{i}$ before re-converging.

Iii-C Risk assessment

As said before, the motivation of the lambda-fields is its ability to compute path integral, hence a risk along a path. For a path $P$ crossing the cells $C = {c_{i}}_{0 : N}$

in order, the probability distribution over the lambda-field is

f (a) = exp (n A λ_{n} - A n - 1 \sum i = 0 λ_{i}) \cdot λ_{n} \cdot exp (- a λ_{n})

(16)

where $n = ⌊ a / A ⌋$ and $⌊ \cdot ⌋$ is the floor function. The variable $a$ denotes the area the robot has already crossed. Figure 4 shows an example of the probability density for a given path on a lambda-field: when the robot goes though high-lambda cells, the cumulative distribution probability quickly increases to one.

Fig. 4: Example of lambda field the robot crosses (in green), with the associated probability distribution $f (a)$ (in blue) and cumulative probability distribution $F (a)$ (in orange).

This can be easily proved as integrating $f (a)$ over a certain path $P$ crossing the cells $C$ gives the probability of encountering at least one collision:

P(coll|P)=∫Pf(a)\difa=1−exp(−Λ(C)).

(17)

We can then define the expectation of a function $r (\cdot)$ over the path:

E[r(X)]=∫Pf(a)r(a)\difa

(18)

The random variable

X

denotes the position (i.e., area) at which the first event ‘collision’ occurs. Most of the time, the cells are small enough to assume that the function

r (\cdot)

is constant inside each cell. Using this assumption, we simplify the above equation to

E [r (X)] = N \sum i = 0 r (A i) \cdot exp (- A i - 1 \sum j = 0 λ_{j}) (1 - exp (- A λ_{i})),

(19)

for a path $P$ going through the cells ${c_{i}}_{0 : N}$ .

In our case, we are interested in the risk of a certain path. In other words, the function $r (\cdot)$ will be the risk encountered at each instant or position.

We chose to model the risk as the force of collision if the collision occurred at the area $a$ , which is

r (a) = m_{R} \cdot v (a),

(20)

where $m_{R}$ is the mass of the robot, and $v (a)$ is its velocity at the area $a$ . One can note that it is quite easy to convert $a$ into the curvilinear abscissa, which is far more convenient to link to the speed. For a robot of width $w$ which has crossed an area $a$ , its curvilinear abscissa $s$ equals to

s = \frac{a}{w} .

(21)

This metric, fast to process, assumes that every obstacle the robot might encounter has an infinite mass. It means that if the robot collides with an obstacle, the resulting collision would lead the robot to stop (i.e., losing a momentum of $m \cdot v$ ).

More complicated metrics can of course be developed. One can think of the maximum of force of collision (i.e., loss/gain of momentum) between the robot and the obstacle of mass $m_{o}$ . This metric is particularly interesting to quantify the fact that, for a same speed, colliding a pedestrian is more ‘risky’ than hitting a car. It needs however to associate a mass to every cell, which can be difficult to do.

Iv Simulation Experiment

As done in [Heiden2017] or [Fulgenzi2007], we demonstrate the validity of our framework in a robot follower scenario. The simulation allows us to perfectly control the position of the robot and the pedestrian it is following, as well as controlling the noise ratio of the sensors. As the other methods of path planning and risk assessment do not take the same risk into account, the comparison is difficult. We indeed do not seek to find the best path, but to ensure that the path the robot takes is below a certain risk limit.

The robot is equipped with a lidar and has no prior knowledge of its environment. The robot wants to follow a pedestrian. It also has no prior knowledge of the pedestrian trajectory but does know its position at each instant. The pedestrian will go through a narrow passage that the robot cannot cross without rising the risk above the maximum. It will hence have to find another way to rejoin the pedestrian.

We discretized the field into cells of size $20 \times $ 20 c m $$ . We assumed that the robot weights $50 k g$ and the sensor has the probability of $p_{h} = 0.99$ and $p_{m} = 0.9999$ to read the right information. The value of $p_{m}$ is intentionally really large since it is almost impossible for a lidar beam to entirely miss an obstacle. It is however far more possible that the lidar returns a hit for a free region (when the beam hit a raindrop, for example) We also choose to model the region of error of the lidar as a disk of area $0.04 m^{2}$ .

We chose to implement the path-planning algorithm presented in [Gerkey2008]. Every second, the robot samples trajectories, parametrized as a velocity $v$ and a rotational velocity $ω$ applied for one second. It then chooses the best trajectory, which is the one that stays the closest to the path (from a global path planning algorithm between the robot and the pedestrian). For each trajectory, we first process its associated upper limit risk using Equation 19. All the trajectories that present an upper limit risk above the maximum risk allowed are discarded. If all of the trajectories are above the risk limit, we allow the robot to have a negative velocity and retry to find a trajectory. If none of the new trajectories is acceptable, the robot choose not to move as it is its only admissible decision. In our case, we chose that the maximum risk is $1 k g m s^{- 1}$ , meaning that we are sure at $95$ percents that the robot will not encounter collision with a force above this maximum.

Figure 5 shows the evolution of the robot across the map. After the first measurement, the robot is not confident enough about the values of lambdas around it and chooses not to move. A second later, enough measurements has been taken and the confidence intervals have been narrowed enough to allow the robot to follow the pedestrian. The pedestrian then goes through a passage too narrow for the robot: every path going in this passage are indeed above the maximum risk allowed. The robot then decides to go around the obstacle as it is its only way to safely reach the pedestrian. The robot can then rise its velocity and meet the pedestrian at the end of the course.

Fig. 5: Evolution of the robot (black boxes with its front represented as a filled triangle) following the pedestrian (in green, with its trajectory in dashed green). The map shows the upper bound $λ_{U}$ for the lambdas. In red, the obstacles. (a) At the beginning, the robot does not have enough confidence to move. (b) After a few seconds, it follows the pedestrian who goes through a narrow passage. (c) The robot tries to go though it, but the risk is to high. The robot then decides to go around the obstacle. (d) The robot meets the pedestrian after the narrow passage.

Fig. 6: The risk taken by the robot (in purple) with its confidence interval (shaded blue). The risk taken by the robot is always below the maximum risk allowed (in dashed red).

Figure 6 shows the risks the robot has taken along its path. We can see three main parts: The first one, between $0 s$ and $7 s$ , is when the robot follows closely the pedestrian. We can see that the robot decides not to move at the first instant, as the risk is too high. The risk lowers as the robot moves, since it takes new measurements every second. The second one, between $7 s$ and $13 s$ , is when the pedestrian goes through the narrow passage. The robot hence tries to follow him while being below its risk limit. We can indeed see that the robot is taking more risks in order to try to follow the pedestrian, which was unsuccessful. The robot then goes around the obstacle in the third part at $13 s$ in order to catch up the pedestrian. The upper risk in the last part is higher since the robot evolves in an area it was not able to measure before, while increasing to the maximum its velocity (hence its risk) in order to reach as fast as possible the pedestrian. Furthermore, we see that the expected risk is zero for the whole navigation, meaning that we expect no collision at all. We are also sure at 95 percent that the force of an unexpected collision is below $1 k g m s^{- 1}$ .

The whole framework is quite light in terms of computation. Indeed, the update of the map only needs to increment two variables per cell ( $h_{i}$ and $m_{i}$ ). The risk assessment also only needs to sum over the cells the robot will cross for the next second, which does not ask many computations for a reasonnable cell size (i.e., $20 \times $ 20 c m $$ in our case).

V Conclusion

In this article, we present a novel representation of occupancy of the environment, called lambda-field. We first derived a way to fill the map, as well as confidence intervals over these values. This representation specifically allows the computation of path integrals. Using the lambda-field, we are able to compute the risk of collision over a path, defined as the force of collision. We then tested the theory in a robot follower situation, where the robot demonstrated its ability to follow the pedestrian while being below the risk limit.

Future works will add dynamic and occluded obstacles, since the robot is not alone in most of the applications.

Acknowledgment

This work was funded by grants from the French program ‘investissement d’avenir’ managed by the National Research Agency (ANR), the European Commission (Auvergne FEDER funds) and the ‘Région Auvergne’ in the framework of the LabEx IMobS 3 (ANR-10-LABX-16-01).

Lambda-Field: A Continuous Counterpart of the Bayesian Occupancy Grid for Risk Assessment

Authors

Lambda-Field: A Continuous Counterpart Of The Bayesian Occupancy Grid For Risk Assessment And Safe Navigation

Dynamic Lambda-Field: A Counterpart of the Bayesian Occupancy Grid for Risk Assessment in Dynamic Environments

Efficient Computation of Collision Probabilities for Safe Motion Planning

When Being Soft Makes You Tough: A Collision Resilient Quadcopter Inspired by Arthropod Exoskeletons

Upper Bounds for Continuous-Time End-to-End Risks in Stochastic Robot Navigation

Upper and Lower Bounds for End-to-End Risks in Stochastic Robot Navigation

Fast Certification of Collision Probability Bounds with Uncertain Convex Obstacles

I Introduction

Ii Related Work